From e3a031bba7e4a4812cdd103ab6afc25e6b4e605f Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Tue, 14 Mar 2023 10:35:53 -0600
Subject: [PATCH] added deferred blocks

---
 src/main/java/frc4388/robot/Robot.java        |  7 ++++++
 .../java/frc4388/robot/subsystems/Arm.java    |  6 ++++-
 .../java/frc4388/utility/DeferredBlock.java   | 23 +++++++++++++++++++
 3 files changed, 35 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc4388/utility/DeferredBlock.java

diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java
index 91a0851..30a8e93 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/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.
    */
diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java
index a7e76c3..4cb95b3 100644
--- a/src/main/java/frc4388/robot/subsystems/Arm.java
+++ b/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;
diff --git a/src/main/java/frc4388/utility/DeferredBlock.java b/src/main/java/frc4388/utility/DeferredBlock.java
new file mode 100644
index 0000000..20d3c57
--- /dev/null
+++ b/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;
+    }
+}