Added the last few subsystems, cleaning up more logging

src/main/java/first/robot/subsystems/Drivetrain.java

@@ -10,10 +10,9 @@ import org.wpilib.command2.Command;
 import org.wpilib.command2.PrintCommand;
 import org.wpilib.command2.SubsystemBase;
 import org.wpilib.driverstation.Alliance;
-import org.wpilib.driverstation.DriverStation;
 import org.wpilib.driverstation.MatchState;
+import org.wpilib.epilogue.Logged;
 import org.wpilib.hardware.imu.OnboardIMU;
-import org.wpilib.hardware.imu.OnboardIMU.MountOrientation;
 import org.wpilib.math.controller.PIDController;
 import org.wpilib.math.estimator.SwerveDrivePoseEstimator;
 import org.wpilib.math.geometry.Pose2d;
@@ -36,6 +35,7 @@ import first.robot.utilities.SwerveModule;
 import first.robot.utilities.Utilities;
 import first.robot.utilities.VisualPose;
 
+@Logged
 public class Drivetrain extends SubsystemBase {
     private SwerveModule frontLeft;
     private SwerveModule frontRight;
@@ -146,11 +146,6 @@ public class Drivetrain extends SubsystemBase {
         frontRight.periodic();
         rearLeft.periodic();
         rearRight.periodic();
-
-        Logger.recordOutput("Drivetrain/Pose", getPose());
-        Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
-        Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
-        Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds());
     }
 
     /**
@@ -268,16 +263,11 @@ public class Drivetrain extends SubsystemBase {
                         targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
                     }
 
-                    Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
-                    Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
-
                     double outputPower = -yawRotationController.calculate(
                         getHeading().getRadians(),
                         targetRotation.getRadians()
                     );
 
-                    Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
-
                     return outputPower;
                 }, 
                 () -> true

src/main/java/first/robot/subsystems/Hood.java

@@ -5,9 +5,9 @@ import java.util.function.DoubleSupplier;
 
 import org.wpilib.command2.Command;
 import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
 import org.wpilib.math.geometry.Pose2d;
 import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
-import org.wpilib.math.util.MathUtil;
 
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
@@ -21,6 +21,7 @@ import first.robot.constants.HoodConstants;
 import first.robot.constants.ShooterConstants.ShooterSpeeds;
 import first.robot.utilities.Utilities;
 
+@Logged
 public class Hood extends SubsystemBase {
     private SparkMax motor;
 
@@ -28,12 +29,7 @@ public class Hood extends SubsystemBase {
 
     private SparkClosedLoopController controller;
 
-    //private Trigger resetTrigger;
-    //private Trigger timerTrigger;
-
-    //private Timer resetTimer;
-
-    private double currentTargetDegrees;
+    private double currentTargetRadians;
 
     public Hood() {
         motor = new SparkMax(
@@ -53,47 +49,35 @@ public class Hood extends SubsystemBase {
 
         controller = motor.getClosedLoopController();
 
-        /*resetTimer = new Timer();
-        resetTimer.reset();
-
-        resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset));
-        resetTrigger.onTrue(new InstantCommand(resetTimer::start));
-        resetTrigger.onFalse(new InstantCommand(() -> {
-            resetTimer.stop();
-            resetTimer.reset();
-        }));
-
-        timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset));
-        timerTrigger.onTrue(new InstantCommand(() -> {
-            encoder.setPosition(0);
-            resetTimer.reset();
-        }));*/
-
-        currentTargetDegrees = HoodConstants.kStartupAngle;
+        currentTargetRadians = HoodConstants.kStartupAngle;
     }
 
-    @Override
-    public void periodic() {
-        SmartDashboard.putNumber(
-            "HoodTargetDegrees", 
-            Math.toDegrees(currentTargetDegrees)
-        );
+    public double getHoodTargetDegrees() {
+        return Math.toDegrees(currentTargetRadians);
+    }
 
-        SmartDashboard.putNumber(
-            "HoodCurrentAngle", 
-            Math.toDegrees(encoder.getPosition())
-        );
+    public double getHoodTargetRadians() {
+        return currentTargetRadians;
+    }
 
-        SmartDashboard.putBoolean(
-            "HoodAtSetpoint", 
-            controller.isAtSetpoint()
-        );
-        
-        Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
-        Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
-        Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
-        Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
-        Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
+    public double getCurrentHoodAngleDegrees() {
+        return Math.toDegrees(encoder.getPosition().get());
+    }
+
+    public double getCurrentHoodAngleRadians() {
+        return encoder.getPosition().get();
+    }
+
+    public double getOutputVoltage() {
+        return motor.getAppliedOutput().get() * motor.getBusVoltage().get();
+    }
+
+    public double getOutputCurrent() {
+        return motor.getOutputCurrent().get();
+    }
+
+    public boolean isHoodAtSetpoint() {
+        return controller.isAtSetpoint().get();
     }
 
     public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
@@ -104,15 +88,13 @@ public class Hood extends SubsystemBase {
             double distance = drivetrainPose.getTranslation()
                 .getDistance(hubPose.getTranslation());
 
-            Logger.recordOutput("Hood/DistanceToHub", distance);
-
             Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
                 
             if(currentSpeeds.isPresent()) {
                 InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
 
                 if(map != null) {
-                    return MathUtil.clamp(map.get(distance), 0, 40);
+                    return Math.clamp(map.get(distance), 0, 40);
                 } else {
                     return 0;
                 }
@@ -122,51 +104,16 @@ public class Hood extends SubsystemBase {
         });
     }
 
-    public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
+    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
         return run(() -> {
-            currentTargetDegrees = degreeAngleSupplier.getAsDouble();
+            currentTargetRadians = radianAngleSupplier.getAsDouble();
 
-            controller.setSetpoint(currentTargetDegrees, ControlType.kPosition);
+            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
         });
     }
 
-    /**
-     * An automated form of resetting the hood position sensing.
-     * 
-     * Run down at the full manual speed (note that this is affected by the 
-     * kMaxManualSpeedMultiplier constant) until the timer trigger becomes true
-     * (i.e. the output current has been above the threshold (kAmpsToTriggerPositionReset) 
-     * for reset for the amount of specified by kTimeAboveThresholdToReset). Once
-     * that returns true, the motor is stopped until the timer trigger switches to false
-     * (i.e. it has reset the position automatically, because that's how it's configured, 
-     * and resets the timer to 0, which makes the timer trigger false)
-     * 
-     * @return A complete Command structure that performs the specified action
-     */
-    /*public Command automatedRezero() {
-        return manualSpeed(() -> -1)
-            .until(timerTrigger)
-            .andThen(
-                stop().until(timerTrigger.negate())
-        );
-    }
-
-    /**
-     * An alternate form of {@link #automatedRezero()} that doesn't rely on the triggers
-     * to reset the hood position to zero. Note that this method doesn't have any time limiting
-     * factor to it, as soon as the current goes above the threshold specified by 
-     * kAmpsToTriggerPositionReset the encoder position will be set to zero
-     * 
-     * @return A complete Command structure that performs the specified action
-     */
-    /*public Command automatedRezeroNoTimer() {
-        return manualSpeed(() -> -1)
-            .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset)
-            .andThen(new InstantCommand(() -> encoder.setPosition(0)));
-    }*/
-
     public Command manualSpeed(DoubleSupplier speed) {
-        currentTargetDegrees = 0;
+        currentTargetRadians = 0;
 
         return run(() -> {
             motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
@@ -176,8 +123,4 @@ public class Hood extends SubsystemBase {
     public Command stop() {
         return manualSpeed(() -> 0);
     }
-
-    public double getTargetDegrees() {
-        return currentTargetDegrees;
-    }
 }

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

@@ -6,6 +6,7 @@ import java.util.function.DoubleSupplier;
 import org.wpilib.command2.Command;
 import org.wpilib.command2.Commands;
 import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
 
 import com.revrobotics.PersistMode;
 import com.revrobotics.RelativeEncoder;
@@ -18,6 +19,7 @@ import com.revrobotics.spark.SparkMax;
 import first.robot.constants.IntakePivotConstants;
 import first.robot.constants.IntakePivotConstants.IntakePivotPosition;
 
+@Logged
 public class IntakePivot extends SubsystemBase {
     private SparkMax leftMotor;
     private SparkMax rightMotor;
@@ -58,13 +60,16 @@ public class IntakePivot extends SubsystemBase {
         encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians());
     }
 
-    @Override
-    public void periodic() {
-        Logger.recordOutput(
-            "IntakePivot/TargetPosition", 
-            currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians());
-        Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition());
-        Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint());
+    public double getTargetPosition() {
+        return currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians();
+    }
+
+    public double getCurrentPosition() {
+        return encoder.getPosition().get();
+    }
+
+    public boolean isAtSetpoint() {
+        return controller.isAtSetpoint().get();
     }
 
     public Command maintainPosition(IntakePivotPosition position) {

src/main/java/first/robot/subsystems/IntakeRoller.java

@@ -2,6 +2,7 @@ package first.robot.subsystems;
 
 import org.wpilib.command2.Command;
 import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
 
 import com.revrobotics.PersistMode;
 import com.revrobotics.ResetMode;
@@ -10,6 +11,7 @@ import com.revrobotics.spark.SparkMax;
 
 import first.robot.constants.IntakeRollerConstants;
 
+@Logged
 public class IntakeRoller extends SubsystemBase {
     private SparkMax leftMotor;
     private SparkMax rightMotor;

src/main/java/first/robot/subsystems/RollerIndexer.java

@@ -0,0 +1,88 @@
+package first.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.RollerIndexerConstants;
+import first.robot.interfaces.IIndexer;
+
+@Logged
+public class RollerIndexer extends SubsystemBase implements IIndexer {
+    private SparkMax motor1;
+    private SparkMax motor2;
+
+    public RollerIndexer() {
+        motor1 = new SparkMax(
+            RollerIndexerConstants.kCANPort,
+            RollerIndexerConstants.kMotor1CANID, 
+            MotorType.kBrushless
+        );
+
+        motor2 = new SparkMax(
+            RollerIndexerConstants.kCANPort,
+            RollerIndexerConstants.kMotor2CANID, 
+            MotorType.kBrushless
+        );
+
+        motor1.configure(
+            RollerIndexerConstants.kMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        motor2.configure(
+            RollerIndexerConstants.kMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+    }
+
+    public Command spinToShooter() {
+        return run(() -> {
+            motor1.setThrottle(RollerIndexerConstants.kMotorSpeed);
+            motor2.setThrottle(-RollerIndexerConstants.kMotorSpeed);
+        });
+    }
+
+    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) {
+        return run(() -> {
+            if(shooterSpeedRPM.getAsDouble() < cutOffRPM) {
+                motor1.setThrottle(0);
+                motor2.setThrottle(0);
+            } else {
+                motor1.setThrottle(RollerIndexerConstants.kMotorSpeed);
+                motor2.setThrottle(-RollerIndexerConstants.kMotorSpeed);
+            }
+        });
+    }
+
+    public Command spinToIntake() {
+        return run(() -> {
+            motor1.setThrottle(-RollerIndexerConstants.kMotorSpeed);
+            motor2.setThrottle(RollerIndexerConstants.kMotorSpeed);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            motor1.setThrottle(0);
+            motor2.setThrottle(0);
+        });
+    }
+
+    public Command instantaneousStop() {
+        return runOnce(() -> {
+            motor1.setThrottle(0);
+            motor2.setThrottle(0);
+        });
+    }
+    
+}

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

@@ -0,0 +1,147 @@
+package first.robot.subsystems;
+
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.math.controller.SimpleMotorFeedforward;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkLowLevel.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.ShooterConstants;
+import first.robot.constants.ShooterConstants.ShooterSpeeds;
+
+public class Shooter extends SubsystemBase {
+    private SparkMax leftMotor;
+    private SparkMax rightMotor;
+
+    private AbsoluteEncoder leftEncoder;
+    private AbsoluteEncoder rightEncoder;
+
+    private SparkClosedLoopController leftClosedLoopController;
+    private SparkClosedLoopController rightClosedLoopController;
+
+    private ShooterSpeeds targetSpeeds;
+    private SimpleMotorFeedforward shooterFFLeft;
+    private SimpleMotorFeedforward shooterFFRight;
+
+    public Shooter() {
+        leftMotor = new SparkMax(
+            ShooterConstants.kCANPort,
+            ShooterConstants.kLeftShooterMotorCANID, 
+            MotorType.kBrushless
+        );
+        
+        rightMotor = new SparkMax(
+            ShooterConstants.kCANPort,
+            ShooterConstants.kRightShooterMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        leftMotor.configure(
+            ShooterConstants.kLeftMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        rightMotor.configure(
+            ShooterConstants.kRightMotorConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        leftEncoder = leftMotor.getAbsoluteEncoder();
+        rightEncoder = rightMotor.getAbsoluteEncoder();
+
+        leftClosedLoopController = leftMotor.getClosedLoopController();
+        rightClosedLoopController = rightMotor.getClosedLoopController();
+
+        // TODO Set this to the initial startup speed
+        targetSpeeds = null;
+
+        shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA);
+        shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA);
+    }
+
+    public double getTargetRPM() {
+        return targetSpeeds == null ? -1 : targetSpeeds.getSpeedRPM();
+    }
+
+    public double getLeftCurrentRPM() {
+        return leftEncoder.getVelocity().get();
+    }
+
+    public double getRightCurrentRPM() {
+        return rightEncoder.getVelocity().get();
+    }
+
+    public double getLeftOutputVoltage() {
+        return leftMotor.getAppliedOutput().get() * leftMotor.getBusVoltage().get();
+    }
+
+    public double getRightOutputVoltage() {
+        return rightMotor.getAppliedOutput().get() * rightMotor.getBusVoltage().get();
+    }
+
+    public boolean isLeftAtSetpoint() {
+        return leftClosedLoopController.isAtSetpoint().get();
+    }
+
+    public boolean isRightAtSetpoint() {
+        return rightClosedLoopController.isAtSetpoint().get();
+    }
+
+    public Command maintainSpeed(ShooterSpeeds speeds) {
+        return run(() -> {
+            targetSpeeds = speeds;
+
+            if(targetSpeeds == null) {
+                leftMotor.disable();
+                rightMotor.disable();
+            } else {
+                leftClosedLoopController.setSetpoint(
+                    targetSpeeds.getSpeedRPM(), 
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFLeft.calculate(targetSpeeds.getSpeedRPM())
+                );
+
+                rightClosedLoopController.setSetpoint(
+                    targetSpeeds.getSpeedRPM(), 
+                    ControlType.kVelocity,
+                    ClosedLoopSlot.kSlot0,
+                    shooterFFRight.calculate(targetSpeeds.getSpeedRPM())
+                );
+            }
+        });
+    }
+
+    public Command manualSpeed(DoubleSupplier speed) {
+        return run(() -> {
+            targetSpeeds = null;
+            
+            leftMotor.setThrottle(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
+            rightMotor.setThrottle(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
+    public double getAverageActualSpeeds() {
+        return (leftEncoder.getVelocity().get() + rightEncoder.getVelocity().get()) / 2;
+    }
+
+    public Optional<ShooterSpeeds> getTargetSpeeds() {
+        return Optional.ofNullable(targetSpeeds);
+    }
+}

src/main/java/first/robot/subsystems/Spindexer.java

@@ -0,0 +1,100 @@
+package first.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import org.wpilib.command2.Command;
+import org.wpilib.command2.SubsystemBase;
+import org.wpilib.epilogue.Logged;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+
+import first.robot.constants.SpindexerConstants;
+import first.robot.interfaces.IIndexer;
+
+@Logged
+public class Spindexer extends SubsystemBase implements IIndexer {
+    private TalonFX spindexerMotor;
+
+    private SparkMax feederMotor;
+
+    private DutyCycleOut spindexerMotorOutput;
+
+    public Spindexer() {
+        spindexerMotor = new TalonFX(
+            SpindexerConstants.kSpindexerMotorCANID,
+            new CANBus(SpindexerConstants.kCANPort)
+        );
+
+        feederMotor = new SparkMax(
+            SpindexerConstants.kCANPort,
+            SpindexerConstants.kFeederMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerCurrentLimitConfig);
+        spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerMotorConfig);
+
+        feederMotor.configure(
+            SpindexerConstants.kFeederConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );
+
+        spindexerMotorOutput = new DutyCycleOut(0);
+    }
+
+    public Command spinToShooter() {
+        return run(() -> {
+            spindexerMotor.setControl(
+                spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
+            );
+            feederMotor.setThrottle(SpindexerConstants.kFeederSpeed);
+        });
+    }
+
+    public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) {
+        return run(() -> {
+            if(shooterSpeedRPM.getAsDouble() < cutOffRPM) {
+                spindexerMotor.setControl(
+                    spindexerMotorOutput.withOutput(0)
+                );
+                feederMotor.setThrottle(0);
+            } else {
+                spindexerMotor.setControl(
+                    spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed)
+                );
+                feederMotor.setThrottle(SpindexerConstants.kFeederSpeed);
+            }
+        });
+    }
+
+    public Command spinToIntake() {
+        return run(() -> {
+            spindexerMotor.setControl(
+                spindexerMotorOutput.withOutput(-SpindexerConstants.kSpindexerSpeed)
+            );
+            feederMotor.setThrottle(-SpindexerConstants.kFeederSpeed);
+        });
+    }
+
+    public Command stop() {
+        return run(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.setThrottle(0);
+        });
+    }
+
+    public Command instantaneousStop() {
+        return runOnce(() -> {
+            spindexerMotor.setControl(spindexerMotorOutput.withOutput(0));
+            feederMotor.setThrottle(0);
+        });
+    }
+    
+}