Coast Mode Elevator Zeroing (╬ಠ益ಠ)

src/main/java/frc/robot/Constants.java

@@ -429,6 +429,13 @@ public static class constElevator {
       ELEVATOR_CONFIG.MotionMagic.MotionMagicAcceleration = 1100;
       ELEVATOR_CONFIG.MotionMagic.MotionMagicExpo_kV = 0.12;
     }
+
+    public static TalonFXConfiguration COAST_MODE_CONFIGURATION = new TalonFXConfiguration();
+    static {
+      COAST_MODE_CONFIGURATION.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+      COAST_MODE_CONFIGURATION.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    }
+
     public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(8.25);
     public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(19);
     public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(34.75);
@@ -442,6 +449,8 @@ public static class constElevator {
     public static final Distance DEADZONE_DISTANCE = Units.Inches.of(1);
     public static final Distance CORAL_INTAKE_HIGHT = Units.Inches.of(0);
 
+    public static final Distance ZEROED_ZONE = Units.Inches.of(1.0);
+
     public static final Distance MAX_HEIGHT = Units.Inches.of(62);
 
     public static final Time ZEROING_TIMEOUT = Units.Seconds.of(3);

src/main/java/frc/robot/Robot.java

@@ -24,6 +24,9 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.constField;
+import frc.robot.commands.Zeroing.ManualZeroAlgaeIntake;
+import frc.robot.commands.Zeroing.ManualZeroElevator;
+import frc.robot.subsystems.Elevator;
 
 @Logged
 public class Robot extends TimedRobot {
@@ -67,6 +70,8 @@ public void disabledInit() {
     bothSubsystemsZeroed = m_robotContainer.allZeroed();
     m_robotContainer.setMegaTag2(false);
 
+    ManualZeroElevator.hasSetCoastMode = false;
+
     if (!hasAutonomousRun) {
       m_robotContainer.manualZeroSubsystems.schedule();
     }

src/main/java/frc/robot/commands/Zeroing/ManualZeroElevator.java

@@ -4,8 +4,11 @@
 
 package frc.robot.commands.Zeroing;
 
+import edu.wpi.first.units.DistanceUnit;
+import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -18,6 +21,8 @@ public class ManualZeroElevator extends Command {
   boolean zeroingSuccess = false;
   Time zeroingTimestamp = Units.Seconds.of(0);
 
+  public static boolean hasSetCoastMode = false;
+
   AngularVelocity lastRotorVelocity = Units.RotationsPerSecond.of(0);
 
   public ManualZeroElevator(Elevator subElevator) {
@@ -33,6 +38,12 @@ public void initialize() {
 
   @Override
   public void execute() {
+
+    if (globalElevator.getElevatorPosition().lte(constElevator.ZEROED_ZONE) && !hasSetCoastMode) {
+      globalElevator.setCoastMode(true);
+      hasSetCoastMode = true;
+    }
+
     // Check if we have raised the elevator above a certain speed
     if (globalElevator.getRotorVelocity().gte(constElevator.MANUAL_ZEROING_START_VELOCITY)
         || globalElevator.attemptingZeroing) {
@@ -67,6 +78,7 @@ public void end(boolean interrupted) {
     if (!interrupted) {
       globalElevator.hasZeroed = true;
       globalElevator.resetSensorPosition(constElevator.ZEROED_POS);
+      globalElevator.setCoastMode(false);
       System.out.println("Elevator Zeroing Successful!!!! Yippee and hooray!!! :3");
     } else {
       System.out.println("Elevator was never zeroed :((( blame eli");

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

@@ -6,12 +6,14 @@
 
 import static edu.wpi.first.units.Units.Inches;
 
+import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix6.controls.Follower;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
 import com.ctre.phoenix6.controls.NeutralOut;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.units.Units;
@@ -73,6 +75,16 @@ public Distance getLastDesiredPosition() {
     return lastDesiredPosition;
   }
 
+  public void setCoastMode(Boolean coastMode) {
+    if (coastMode) {
+      rightMotorLeader.getConfigurator().apply(constElevator.COAST_MODE_CONFIGURATION);
+      leftMotorFollower.getConfigurator().apply(constElevator.COAST_MODE_CONFIGURATION);
+    } else {
+      rightMotorLeader.getConfigurator().apply(constElevator.ELEVATOR_CONFIG);
+      leftMotorFollower.getConfigurator().apply(constElevator.ELEVATOR_CONFIG);
+    }
+  }
+
   public boolean isRotorVelocityZero() {
     return getRotorVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01);
   }