changed wrist homing

src/main/java/frc/robot/config/CompConfig.java

@@ -68,13 +68,16 @@ class CompConfig {
           new ClimberConfig(
               20,
               21,
+              0.0,
               false,
               4,
               0.0,
               0.0,
               0.0,
               0.0,
               0.0,
+              0.0,
+              0.0,
               new TalonFXConfiguration()
                   .withSlot0(new Slot0Configs().withKP(8))
                   .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(17.0 / 1.0))
@@ -111,10 +114,7 @@ class CompConfig {
                           .withInverted(InvertedValue.Clockwise_Positive)
                           .withNeutralMode(NeutralModeValue.Brake)),
               new CurrentLimitsConfigs().withSupplyCurrentLimit(40),
-              -0.05,
-              10,
               Rotation2d.fromDegrees(-14),
-              4,
               Rotation2d.fromDegrees(-13.5),
               Rotation2d.fromDegrees(115),
               distanceToAngleTolerance -> {

src/main/java/frc/robot/config/PracticeConfig.java

@@ -66,13 +66,16 @@ class PracticeConfig {
           new ClimberConfig(
               20,
               21,
+              0.0,
               false,
               4,
               0.0,
               0.0,
               0.0,
               0.0,
               0.0,
+              0.0,
+              0.0,
               new TalonFXConfiguration()
                   .withSlot0(new Slot0Configs().withKP(8))
                   .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(17.0 / 1.0))
@@ -109,10 +112,7 @@ class PracticeConfig {
                           .withInverted(InvertedValue.Clockwise_Positive)
                           .withNeutralMode(NeutralModeValue.Brake)),
               new CurrentLimitsConfigs().withSupplyCurrentLimit(40),
-              -0.05,
-              10,
               Rotation2d.fromDegrees(-18),
-              4,
               Rotation2d.fromDegrees(-18),
               Rotation2d.fromDegrees(95),
               distanceToAngleTolerance -> {

src/main/java/frc/robot/config/RobotConfig.java

@@ -36,13 +36,16 @@ public record ShooterConfig(
   public record ClimberConfig(
       int mainMotorID,
       int followerMotorID,
+      double accelerationTolerance,
       boolean opposeMasterDirection,
       int currentTaps,
       double homingCurrentThreshold,
-      double homingEndPosition,
       double homingVoltage,
       double minPosition,
       double maxPosition,
+      double idlePosition,
+      double raisedPosition,
+      double hangingPosition,
       TalonFXConfiguration motorConfig) {}
 
   public record IntakeConfig(int motorID, int sensorID, TalonFXConfiguration motorConfig) {}
@@ -55,10 +58,7 @@ public record WristConfig(
       int motorID,
       TalonFXConfiguration motorConfig,
       CurrentLimitsConfigs strictCurrentLimits,
-      double homingVoltage,
-      double homingCurrentThreshold,
       Rotation2d homingEndPosition,
-      int currentTaps,
       Rotation2d minAngle,
       Rotation2d maxAngle,
       Consumer<InterpolatingDoubleTreeMap> distanceToAngleTolerance,

src/main/java/frc/robot/elevator/ElevatorSubsystem.java

@@ -86,8 +86,6 @@ public void robotPeriodic() {
       if (preMatchHomingOccured) {
             double homedPosition = homingEndPosition + (getHeight() - lowestSeenHeight);
             motor.setPosition(inchesToRotations(homedPosition));
-
-            preMatchHomingOccured = true;
             homingState = HomingState.HOMED;
           }
         break;

src/main/java/frc/robot/wrist/WristSubsystem.java

@@ -6,7 +6,9 @@
 
 import com.ctre.phoenix6.controls.CoastOut;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.StaticBrake;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -20,17 +22,17 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
 public class WristSubsystem extends LifecycleSubsystem {
-  private static final Rotation2d PRE_MATCH_HOMING_MIN_MOVEMENT = Rotation2d.fromDegrees(60);
   private final TalonFX motor;
   private final LoggedDashboardNumber ntAngle =
       new LoggedDashboardNumber("Wrist/AngleOverride", -1);
-  private final MotionMagicVoltage positionRequest =
-      new MotionMagicVoltage(WristPositions.STOWED.getRotations()).withEnableFOC(true);
+  private final PositionVoltage positionRequest =
+      new PositionVoltage(WristPositions.STOWED.getRotations()).withEnableFOC(true);
+
   private final StaticBrake brakeNeutralRequest = new StaticBrake();
   private final CoastOut coastNeutralRequest = new CoastOut();
+
   private HomingState homingState = HomingState.PRE_MATCH_HOMING;
-  private final LinearFilter currentFilter =
-      LinearFilter.movingAverage(RobotConfig.get().wrist().currentTaps());
+
   private static final InterpolatingDoubleTreeMap speakerDistanceToAngle =
       new InterpolatingDoubleTreeMap();
   private static final InterpolatingDoubleTreeMap floorSpotDistanceToAngle =
@@ -39,7 +41,6 @@ public class WristSubsystem extends LifecycleSubsystem {
       new InterpolatingDoubleTreeMap();
 
   private Rotation2d lowestSeenAngle = new Rotation2d();
-  private Rotation2d highestSeenAngle = new Rotation2d();
   private int slot = 0;
   private Rotation2d TOLERANCE = Rotation2d.fromDegrees(5);
 
@@ -65,64 +66,44 @@ public void disabledPeriodic() {
     if (currentAngle.getDegrees() < lowestSeenAngle.getDegrees()) {
       lowestSeenAngle = currentAngle;
     }
-
-    if (currentAngle.getDegrees() > highestSeenAngle.getDegrees()) {
-      highestSeenAngle = currentAngle;
-    }
   }
 
   @Override
   public void robotPeriodic() {
-    double rawCurrent = motor.getStatorCurrent().getValueAsDouble();
-    double filteredCurrent = currentFilter.calculate(rawCurrent);
-
-    Logger.recordOutput("Wrist/FilteredCurrent", filteredCurrent);
-
     switch (homingState) {
       case NOT_HOMED:
-        if (DriverStation.isDisabled()) {
-          if (rangeOfMotionSeen()) {
-            motor.setControl(brakeNeutralRequest);
-          } else {
-            motor.setControl(coastNeutralRequest);
-          }
-        }
+        motor.setControl(coastNeutralRequest);
         break;
       case PRE_MATCH_HOMING:
         if (DriverStation.isDisabled()) {
-          if (rangeOfMotionSeen()) {
-            motor.setControl(brakeNeutralRequest);
-          } else {
-            motor.setControl(coastNeutralRequest);
-          }
+          motor.setControl(coastNeutralRequest);
         } else {
           motor.setControl(brakeNeutralRequest);
 
-          // If we need to require the range of motion to be seen, add " && rangeOfMotionSeen()"
-
           if (!preMatchHomingOccured) {
-            // homingEndPosition + (currentAngle - minAngle)
             Rotation2d homingEndPosition = RobotConfig.get().wrist().homingEndPosition();
             Rotation2d homedAngle =
                 Rotation2d.fromDegrees(
                     homingEndPosition.getDegrees()
                         + (getAngle().getDegrees() - lowestSeenAngle.getDegrees()));
             motor.setPosition(homedAngle.getRotations());
+
             preMatchHomingOccured = true;
             homingState = HomingState.HOMED;
           }
         }
         break;
       case MID_MATCH_HOMING:
-        motor.set(RobotConfig.get().wrist().homingVoltage());
-
-        if (filteredCurrent > RobotConfig.get().wrist().homingCurrentThreshold()) {
-          homingState = HomingState.HOMED;
-          goalAngle = WristPositions.STOWED;
-
-          motor.setPosition(RobotConfig.get().wrist().homingEndPosition().getRotations());
-        }
+        if (preMatchHomingOccured) {
+            Rotation2d homingEndPosition = RobotConfig.get().wrist().homingEndPosition();
+            Rotation2d homedAngle =
+                Rotation2d.fromDegrees(
+                    homingEndPosition.getDegrees()
+                        + (getAngle().getDegrees() - lowestSeenAngle.getDegrees()));
+            motor.setPosition(homedAngle.getRotations());
 
+            homingState = HomingState.HOMED;
+          }
         break;
       case HOMED:
         Rotation2d usedGoalAngle =
@@ -148,11 +129,6 @@ public void robotPeriodic() {
     Logger.recordOutput("Wrist/ControlMode", motor.getControlMode().toString());
   }
 
-  public boolean rangeOfMotionSeen() {
-    return highestSeenAngle.getDegrees() - lowestSeenAngle.getDegrees()
-        >= PRE_MATCH_HOMING_MIN_MOVEMENT.getDegrees();
-  }
-
   public void setAngle(Rotation2d angle) {
     angle = clampAngle(angle);