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

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

@@ -17,6 +17,8 @@ public class HoodConstants {
     // TODO Real Values
     public static final int kMotorCANID = 0;
 
+    public static final double kConversionFactor = 0;
+
     public static final double kP = 0;
     public static final double kI = 0;
     public static final double kD = 0;
@@ -26,6 +28,12 @@ public class HoodConstants {
     public static final double kStartupAngle = 0;
     public static final double kMaxManualSpeedMultiplier = .5;
 
+    public static final double kAmpsToTriggerPositionReset = 10;
+
+    // TODO This is just barely longer than the default frame time for output current information
+    // Should this be longer?
+    public static final double kTimeAboveThresholdToReset = .25;
+
     public static final int kCurrentLimit = 15;
 
     public static final boolean kInverted = false;
@@ -45,12 +53,13 @@ public class HoodConstants {
             .idleMode(kIdleMode)
             .inverted(kInverted)
             .smartCurrentLimit(kCurrentLimit);
+        kConfig.encoder
+            .positionConversionFactor(kConversionFactor)
+            .velocityConversionFactor(kConversionFactor / 60);
         kConfig.closedLoop
-            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
             .pid(kP, kI, kD)
             .outputRange(-1, 1)
-            .positionWrappingEnabled(true)
-            .positionWrappingInputRange(0, Math.PI * 2)
             .feedForward
                 .sva(kS, kV, kA);
 

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() {
+    public double getTargetDegrees() {
-        return currentTargetRadians;
+        return currentTargetDegrees;
     }
 }