Adjustments to the hood now that I know we're going with relative not absolute encoders

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

@@ -5,51 +5,84 @@ import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
 import com.revrobotics.spark.SparkClosedLoopController;
 import com.revrobotics.spark.SparkMax;
 import com.revrobotics.spark.SparkBase.ControlType;
 import com.revrobotics.spark.SparkLowLevel.MotorType;
 
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.HoodConstants;
 
 public class Hood extends SubsystemBase {
     private SparkMax motor;
 
-    private AbsoluteEncoder encoder;
+    private RelativeEncoder encoder;
 
     private SparkClosedLoopController controller;
 
-    private double currentTargetRadians;
+    private Trigger resetTrigger;
+    private Trigger timerTrigger;
+
+    private Timer resetTimer;
+
+    private double currentTargetDegrees;
 
     public Hood() {
         motor = new SparkMax(HoodConstants.kMotorCANID, MotorType.kBrushless);
 
-        encoder = motor.getAbsoluteEncoder();
+        motor.configure(
+            HoodConstants.kConfig, 
+            ResetMode.kResetSafeParameters, 
+            PersistMode.kPersistParameters
+        );   
+
+        encoder = motor.getEncoder();
 
         controller = motor.getClosedLoopController();
 
-        currentTargetRadians = HoodConstants.kStartupAngle;
+        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() {
-        Logger.recordOutput("Hood/CurrentTarget", currentTargetRadians);
+        Logger.recordOutput("Hood/CurrentTarget", currentTargetDegrees);
         Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
         Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
     }
 
-    public Command trackToAngle(DoubleSupplier radianAngleSupplier) {
+    public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
         return run(() -> {
-            currentTargetRadians = radianAngleSupplier.getAsDouble();
+            currentTargetDegrees = degreeAngleSupplier.getAsDouble();
 
-            controller.setSetpoint(currentTargetRadians, ControlType.kPosition);
+            controller.setSetpoint(currentTargetDegrees, ControlType.kPosition);
         });
     }
 
     public Command manualSpeed(DoubleSupplier speed) {
-        currentTargetRadians = 0;
+        currentTargetDegrees = 0;
 
         return run(() -> {
             motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
@@ -60,7 +93,7 @@ public class Hood extends SubsystemBase {
         return manualSpeed(() -> 0);
     }
 
-    public double getTargetRadians() {
-        return currentTargetRadians;
+    public double getTargetDegrees() {
+        return currentTargetDegrees;
     }
 }