added deferred blocks

src/main/java/frc4388/robot/Robot.java

@@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
 
 import frc4388.robot.subsystems.Location;
@@ -113,6 +114,12 @@ public class Robot extends TimedRobot {
     // System.out.println("hi from disabled");
   }
 
+  @Override
+  public void disabledExit() {
+      DeferredBlock.execute();
+      super.disabledExit();
+  }
+
   /**
    * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
    */

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -10,6 +10,7 @@ import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import frc4388.robot.Constants.ArmConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
+import frc4388.utility.DeferredBlock;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -30,6 +31,9 @@ public class Arm extends SubsystemBase {
         tele.configFactoryDefault();
         m_pivot.configFactoryDefault();
 
+        // * Example of deferred code
+        new DeferredBlock(() -> resetTeleSoftLimit());
+
         // TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
         // pivotConfig.slot0.kP = 0.5;//ArmConstants.kP;
         // pivotConfig.slot0.kI = 0.0;//ArmConstants.kI;
@@ -41,7 +45,7 @@ public class Arm extends SubsystemBase {
 
         // m_pivot.configAllSettings(pivotConfig);
 
-        resetTeleSoftLimit();
+        // resetTeleSoftLimit();
 
         CANCoderConfiguration config = new CANCoderConfiguration();
         config.magnetOffsetDegrees = ArmConstants.OFFSET;

src/main/java/frc4388/utility/DeferredBlock.java

@@ -0,0 +1,23 @@
+package frc4388.utility;
+
+import java.util.ArrayList;
+
+public class DeferredBlock {
+    private static ArrayList<Runnable> m_blocks = new ArrayList<>();
+    private static boolean             m_hasRun = false;
+
+    public DeferredBlock(Runnable block) {
+        m_blocks.add(block);
+    }
+
+    public static void execute() {
+        if (m_hasRun) return;
+
+        for (Runnable block : m_blocks) {
+            block.run();
+        }
+
+        m_blocks.clear(); // for garbage collection
+        m_hasRun = true;
+    }
+}