fix elevator subsystem units

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

@@ -18,13 +18,13 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.config.RobotConfig.ClimberConfig;
+import frc.robot.config.RobotConfig.ElevatorConfig;
 import frc.robot.config.RobotConfig.IMUConfig;
 import frc.robot.config.RobotConfig.IntakeConfig;
 import frc.robot.config.RobotConfig.LightsConfig;
 import frc.robot.config.RobotConfig.ShooterConfig;
 import frc.robot.config.RobotConfig.ShoulderConfig;
 import frc.robot.config.RobotConfig.SwerveConfig;
-import frc.robot.config.RobotConfig.ElevatorConfig;
 
 class CompConfig {
   public static final RobotConfig competitionBot =
@@ -127,7 +127,7 @@ class CompConfig {
                 floorSpotDistanceToAngle.put(4.8, 20.0);
                 floorSpotDistanceToAngle.put(15.0, 10.0);
               }),
-              new ElevatorConfig(
+          new ElevatorConfig(
               0,
               new TalonFXConfiguration()
                   .withSlot0(
@@ -142,35 +142,22 @@ class CompConfig {
                           .withKP(0.0))
                   .withMotionMagic(
                       new MotionMagicConfigs()
-                          .withMotionMagicAcceleration(0.0)
-                          .withMotionMagicCruiseVelocity(0.0)
+                          .withMotionMagicAcceleration(2.0)
+                          .withMotionMagicCruiseVelocity(1.5)
                           .withMotionMagicJerk(0))
-                  .withFeedback(
-                      new FeedbackConfigs()
-                          .withSensorToMechanismRatio(
-                              0.0))
-                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(0))
+                  .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(0))
+                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(40))
                   .withMotorOutput(
                       new MotorOutputConfigs()
                           .withInverted(InvertedValue.Clockwise_Positive)
                           .withNeutralMode(NeutralModeValue.Brake)),
-              new CurrentLimitsConfigs().withSupplyCurrentLimit(0),
-              0.0,
+              new CurrentLimitsConfigs().withSupplyCurrentLimit(40),
+              -0.05,
+              0,
               0,
-              Rotation2d.fromDegrees(0),
               4,
-              Rotation2d.fromDegrees(0),
-              Rotation2d.fromDegrees(0),
-              distanceToAngleTolerance -> {
-                distanceToAngleTolerance.put(0.0, 0.0);
-                distanceToAngleTolerance.put(0.0, 0.0);
-              },
-              speakerDistanceToAngle -> {
-                speakerDistanceToAngle.put(0.0, 0.0);
-              },
-              floorSpotDistanceToAngle -> {
-                floorSpotDistanceToAngle.put(0.0, 0.0);
-              }),
+              0.0,
+              0.0),
           new IntakeConfig(
               16,
               22,

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

@@ -17,14 +17,13 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.config.RobotConfig.ClimberConfig;
+import frc.robot.config.RobotConfig.ElevatorConfig;
 import frc.robot.config.RobotConfig.IMUConfig;
 import frc.robot.config.RobotConfig.IntakeConfig;
 import frc.robot.config.RobotConfig.LightsConfig;
 import frc.robot.config.RobotConfig.ShooterConfig;
 import frc.robot.config.RobotConfig.ShoulderConfig;
 import frc.robot.config.RobotConfig.SwerveConfig;
-import frc.robot.config.RobotConfig.ElevatorConfig;
-
 
 class PracticeConfig {
   public static final RobotConfig config =
@@ -117,7 +116,7 @@ class PracticeConfig {
                 floorSpotDistanceToAngle.put(3.4, 60.0);
                 floorSpotDistanceToAngle.put(4.8, 50.0);
               }),
-              new ElevatorConfig(
+          new ElevatorConfig(
               0,
               new TalonFXConfiguration()
                   .withSlot0(
@@ -135,10 +134,7 @@ class PracticeConfig {
                           .withMotionMagicAcceleration(2.0)
                           .withMotionMagicCruiseVelocity(1.5)
                           .withMotionMagicJerk(0))
-                  .withFeedback(
-                      new FeedbackConfigs()
-                          .withSensorToMechanismRatio(
-                              0))
+                  .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(0))
                   .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(40))
                   .withMotorOutput(
                       new MotorOutputConfigs()
@@ -147,22 +143,10 @@ class PracticeConfig {
               new CurrentLimitsConfigs().withSupplyCurrentLimit(40),
               -0.05,
               0,
-              Rotation2d.fromDegrees(0),
+              0,
               4,
-              Rotation2d.fromDegrees(0),
-              Rotation2d.fromDegrees(0),
-              distanceToAngleTolerance -> {
-                distanceToAngleTolerance.put(0.0, 0.0);
-                distanceToAngleTolerance.put(0.0, 0.0);
-              },
-              speakerDistanceToAngle -> {
-                speakerDistanceToAngle.put(0.0, 0.0);
-
-              },
-              floorSpotDistanceToAngle -> {
-                floorSpotDistanceToAngle.put(0.0, 0.0);
-
-              }),
+              0.0,
+              0.0),
           new IntakeConfig(
               16,
               22,
@@ -192,10 +176,5 @@ class PracticeConfig {
                 distanceToAngleTolerance.put(1.0, 2.5);
                 distanceToAngleTolerance.put(1.0, 2.5);
               }),
-
-
-
-              new LightsConfig(23));
-
-
+          new LightsConfig(23));
 }

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

@@ -21,10 +21,7 @@ public record RobotConfig(
     IntakeConfig intake,
     SwerveConfig swerve,
     IMUConfig imu,
-    LightsConfig lights
-
-    ) {
-
+    LightsConfig lights) {
 
   public record ShooterConfig(
       int bottomMotorID,
@@ -59,19 +56,16 @@ public record ShoulderConfig(
       Consumer<InterpolatingDoubleTreeMap> speakerShotAngles,
       Consumer<InterpolatingDoubleTreeMap> floorShotAngles) {}
 
-      public record ElevatorConfig(
-      int rightMotorID,
+  public record ElevatorConfig(
+      int motorID,
       TalonFXConfiguration motorConfig,
       CurrentLimitsConfigs strictCurrentLimits,
       double homingVoltage,
       double homingCurrentThreshold,
       double homingEndPosition,
       int currentTaps,
-      Rotation2d minAngle,
-      Rotation2d maxAngle,
-      Consumer<InterpolatingDoubleTreeMap> distanceToAngleTolerance,
-      Consumer<InterpolatingDoubleTreeMap> speakerShotAngles,
-      Consumer<InterpolatingDoubleTreeMap> floorShotAngles) {}
+      double minHeight,
+      double maxHeight) {}
 
   public record IMUConfig(
       int deviceID, Consumer<InterpolatingDoubleTreeMap> distanceToAngleTolerance) {}

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

@@ -1,3 +1,7 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
 package frc.robot.elevator;
 
 public class ElevatorPositions {

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

@@ -6,11 +6,9 @@
 
 import com.ctre.phoenix6.controls.CoastOut;
 import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC;
-import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.StaticBrake;
 import com.ctre.phoenix6.hardware.TalonFX;
 import edu.wpi.first.math.filter.LinearFilter;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.config.RobotConfig;
 import frc.robot.util.HomingState;
@@ -50,11 +48,11 @@ public class ElevatorSubsystem extends LifecycleSubsystem {
   private int slot = 0;
 
   public ElevatorSubsystem(TalonFX motor) {
-
     super(SubsystemPriority.ELEVATOR);
-    this.motor = motor;
+
     motor.getConfigurator().apply(RobotConfig.get().elevator().motorConfig());
 
+    this.motor = motor;
   }
 
   @Override
@@ -85,7 +83,7 @@ public void robotPeriodic() {
             // homingEndPosition + (currentAngle - minAngle)
             double homingEndPosition = RobotConfig.get().elevator().homingEndPosition();
             double homedPosition = homingEndPosition + (getHeight() - lowestSeenHeight);
-            motor.setPosition(homedPosition);
+            motor.setPosition(inchesToRotations(homedPosition));
 
             preMatchHomingOccured = true;
             homingState = HomingState.HOMED;
@@ -99,16 +97,18 @@ public void robotPeriodic() {
           homingState = HomingState.HOMED;
           goalHeight = ElevatorPositions.STOWED;
 
-          motor.setPosition(RobotConfig.get().elevator().homingEndPosition());
+          motor.setPosition(inchesToRotations(RobotConfig.get().elevator().homingEndPosition()));
         }
         break;
       case HOMED:
-        double usedGoalPosition = ntposition.get() == -1 ? clampHeight(goalHeight) : ntposition.get();
+        double usedGoalPosition =
+            ntposition.get() == -1 ? clampHeight(goalHeight) : ntposition.get();
 
         slot = goalHeight == minHeight ? 1 : 0;
-        Logger.recordOutput("Elevator/UsedGoalAngle", usedGoalPosition);
+        Logger.recordOutput("Elevator/UsedGoalPosition", usedGoalPosition);
 
-        motor.setControl(positionRequest.withSlot(slot).withPosition(usedGoalPosition));
+        motor.setControl(
+            positionRequest.withSlot(slot).withPosition(inchesToRotations(usedGoalPosition)));
 
         break;
     }
@@ -118,10 +118,9 @@ public boolean rangeOfMotionSeen() {
     return highestSeenHeight - lowestSeenHeight >= PRE_MATCH_HOMING_MIN_MOVEMENT;
   }
 
-  public void setHeight(double newHeight) {
-      goalHeight = clampHeight(newHeight);
-    }
-
+  public void setGoalHeight(double newHeight) {
+    goalHeight = clampHeight(newHeight);
+  }
 
   public double getHeight() {
     return rotationsToInches(motor.getPosition().getValueAsDouble());
@@ -131,19 +130,25 @@ public boolean atGoal() {
     return Math.abs(getHeight() - goalHeight) < TOLERANCE;
   }
 
-  // Tune later
+  public boolean atGoal(double inches) {
+    return Math.abs(getHeight() - inches) < TOLERANCE;
+  }
+
+  // Tune the radius in inches later
   private double rotationsToInches(double rotations) {
-    return rotations * 0;
+    return rotations * (2 * Math.PI * 0);
+  }
+
+  private double inchesToRotations(double inches) {
+    return inches / (2 * Math.PI * 0);
   }
 
   private static double clampHeight(double height) {
     if (height < minHeight) {
       height = minHeight;
-
     } else if (height > maxHeight) {
       height = maxHeight;
     }
-
     return height;
   }
 }

src/main/java/frc/robot/util/scheduling/SubsystemPriority.java

@@ -15,7 +15,7 @@ public enum SubsystemPriority {
   IMU(10),
   SHOOTER(10),
   SHOULDER(10),
-  ELEVATOR(10),//Maybe needs to be different priority
+  ELEVATOR(10), // Maybe needs to be different priority
   VISION(10),
   LOCALIZATION(10),
   LIGHTS(10),