A bunch of random work on Subsystems, lots of work left to do

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

@@ -0,0 +1,183 @@
+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.geometry.Pose2d;
+import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap;
+import org.wpilib.math.util.MathUtil;
+
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+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.HoodConstants;
+import first.robot.constants.ShooterConstants.ShooterSpeeds;
+import first.robot.utilities.Utilities;
+
+public class Hood extends SubsystemBase {
+    private SparkMax motor;
+
+    private RelativeEncoder encoder;
+
+    private SparkClosedLoopController controller;
+
+    //private Trigger resetTrigger;
+    //private Trigger timerTrigger;
+
+    //private Timer resetTimer;
+
+    private double currentTargetDegrees;
+
+    public Hood() {
+        motor = new SparkMax(
+            HoodConstants.kCANPort, 
+            HoodConstants.kMotorCANID, 
+            MotorType.kBrushless
+        );
+
+        motor.configure(
+            HoodConstants.kConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );   
+
+        encoder = motor.getEncoder();
+        encoder.setPosition(HoodConstants.kStartupAngle);
+
+        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;
+    }
+
+    @Override
+    public void periodic() {
+        SmartDashboard.putNumber(
+            "HoodTargetDegrees", 
+            Math.toDegrees(currentTargetDegrees)
+        );
+
+        SmartDashboard.putNumber(
+            "HoodCurrentAngle", 
+            Math.toDegrees(encoder.getPosition())
+        );
+
+        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 Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
+        return trackToAngle(() -> {
+            Pose2d drivetrainPose = drivetrain.getPose();
+            Pose2d hubPose = Utilities.getHubPose();
+
+            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);
+                } else {
+                    return 0;
+                }
+            } else {
+                return 0;
+            }
+        });
+    }
+
+    public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
+        return run(() -> {
+            currentTargetDegrees = degreeAngleSupplier.getAsDouble();
+
+            controller.setSetpoint(currentTargetDegrees, 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;
+
+        return run(() -> {
+            motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
+        });
+    }
+
+    public Command stop() {
+        return manualSpeed(() -> 0);
+    }
+
+    public double getTargetDegrees() {
+        return currentTargetDegrees;
+    }
+}