Added the last few subsystems, cleaning up more logging

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;
-    }
 }