Pulled in auto changes only from 111

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2023.4.2"
+    id "edu.wpi.first.GradleRIO" version "2023.4.3"
 }
 
 sourceCompatibility = JavaVersion.VERSION_11

src/main/java/org/wildstang/framework/auto/AutoManager.java

@@ -23,6 +23,8 @@ public class AutoManager {
     private boolean programFinished;
     private SendableChooser<AutoProgram> chooser;
     private SendableChooser<Boolean> lockinChooser;
+    private boolean hasStarted;
+    private boolean lastLock;
 
     /**
      * Loads in AutoPrograms and adds selectors to the SmartDashboard.
@@ -48,16 +50,72 @@ public void update() {
             programFinished = false;
             startSleeper();
         }
+        // if an auto program hasn't started yet and only once after the lock state changes
+        else if (!hasStarted) {
+            boolean locked = lockinChooser.getSelected();
+            if (locked != lastLock) {
+                Log.info("Lock state changed to: " + locked);
+                lastLock = locked;
+                // attempt to preload the selected program
+                preloadLockedProgram();
+            }
+        }
+
+        // display currently loading and running program
+        if (runningProgram != null && runningProgram.areStepsDefined()) {
+            SmartDashboard.putString("Preloaded Autonomous Program", runningProgram.toString());
+        }
+        else {
+            SmartDashboard.putString("Preloaded Autonomous Program", "");
+        }
+        if (runningProgram != null && hasStarted) {
+            SmartDashboard.putString("Running Autonomous Program", runningProgram.toString());
+        }
+        else {
+            SmartDashboard.putString("Running Autonomous Program", "");
+        }
+
         runningProgram.update();
         if (runningProgram.isFinished()) {
             programFinished = true;
         }
     }
 
+    /**
+     * Preloads the currently selected program from SmartDashboard if locked in.
+     */
+    public void preloadLockedProgram() {
+        if (lockinChooser.getSelected()) {
+            preloadProgram(chooser.getSelected());
+        }
+    }
+
+    /**
+     * Defines steps for the given program.
+     * @param program New program to preload.
+     */
+    private void preloadProgram(AutoProgram program) {
+        if (runningProgram != null && runningProgram.areStepsDefined()) {
+            // if this is a new program, clear out old program's steps
+            if (runningProgram != program) {
+                runningProgram.cleanup();
+            }
+            else {
+                Log.warn("Steps already defined for " + program.toString());
+                return;
+            }
+        }
+
+        runningProgram = program;
+        program.defineSteps();
+        Log.info("Preloading auto program: " + program.toString());
+    }
+
     /**
      * Starts the currently selected program from SmartDashboard if locked in.
      */
     public void startCurrentProgram() {
+        hasStarted = true;
         if (lockinChooser.getSelected()) {
             startProgram(chooser.getSelected());
         } else {
@@ -80,14 +138,15 @@ private void startProgram(AutoProgram program) {
         runningProgram = program;
         program.initialize();
         Log.info("Starting auto program: " + program.toString());
-        SmartDashboard.putString("Running Autonomous Program", program.toString());
     }
 
     /**
      * Resets the current program and all states.
      */
     public void init() {
         programFinished = false;
+        hasStarted = false;
+        lastLock = false;
         if (runningProgram != null) {
             runningProgram.cleanup();
         }
@@ -142,4 +201,20 @@ public void addProgram(AutoProgram program) {
     public boolean isLockedIn(){
         return lockinChooser.getSelected();
     }
+
+    /**
+     * Ends the autonomous period, prevents it from continuing into tele.
+     */
+    public void endPeriod() {
+        // prevent any more programs from being preloaded
+        hasStarted = true;
+
+        // tell all programs to clear out their steps
+        for (AutoProgram program : programs) {
+            program.cleanup();
+        }
+
+        // trigger Sleeper program on next update
+        programFinished = true;
+    }
 }

src/main/java/org/wildstang/framework/auto/AutoProgram.java

@@ -17,7 +17,7 @@ public abstract class AutoProgram {
 
     protected final List<AutoStep> programSteps = new ArrayList<>();
     protected int currentStep;
-    protected boolean finishedPreviousStep, finished;
+    protected boolean finishedPreviousStep, finished, programStarted;
 
     /**
      * Use this method to set the steps for this program.
@@ -31,12 +31,17 @@ public abstract class AutoProgram {
      * Collects the defined steps and starts the auto program.
      */
     public void initialize() {
-        defineSteps();
+        // prevent the steps from being double defined, AutoManager attempts to pre-define steps
+        if (programSteps.isEmpty())
+        {
+            defineSteps();
+        }
         loadStopPosition();
         currentStep = 0;
         finishedPreviousStep = false;
         finished = false;
         startStep(currentStep);
+        programStarted = true;
     }
 
     /**
@@ -46,6 +51,14 @@ public void cleanup() {
         programSteps.clear();
     }
 
+    /**
+     * Determines if the program's steps have been defined.
+     * @return Returns true if there are any steps.
+     */
+    public boolean areStepsDefined() {
+        return !programSteps.isEmpty();
+    }
+
     /**
      * Update the current running step and move on if necessary.
      */
@@ -64,7 +77,7 @@ public void update() {
                 startStep(currentStep);
             }
         }
-        if (programSteps.size() > currentStep) {
+        if (programSteps.size() > currentStep && programStarted) {
             AutoStep step = programSteps.get(currentStep); // Prevent errors caused by mistyping.
             SmartDashboard.putString("Current auto step", step.toString());
             step.update();

src/main/java/org/wildstang/hardware/roborio/outputs/WsSparkMax.java

@@ -254,6 +254,30 @@ public void initClosedLoop(double P, double I, double D, double FF, AbsoluteEnco
         isUsingController = true;
         slotID = 0;
     }
+    /**
+     * Sets up closed loop control for the motor
+     * @param P the P value
+     * @param I the I value
+     * @param D the D value
+     * @param FF the feed forward constant
+     */
+    public void initClosedLoop(double P, double I, double D, double FF, AbsoluteEncoder absEncoder, boolean isWrapped){
+        controller = motor.getPIDController();
+        controller.setP(P, 0);
+        controller.setI(I, 0);
+        controller.setD(D, 0);
+        controller.setFF(FF, 0);
+        controller.setFeedbackDevice(absEncoder);
+        controller.setPositionPIDWrappingEnabled(isWrapped);
+        controller.setPositionPIDWrappingMinInput(0.0);
+        controller.setPositionPIDWrappingMaxInput(360.0);
+        isUsingController = true;
+        slotID = 0;
+    }
+
+    public SparkMaxPIDController getPIDController(){
+        return motor.getPIDController();
+    }
 
     /*
      * Adds a closed loop control slot for the sparkmax

src/main/java/org/wildstang/year2023/robot/Robot.java

@@ -20,7 +20,6 @@
 public class Robot extends TimedRobot {
 
     Core core;
-    private boolean AutoFirstRun = true;
     private SendableChooser<LogLevel> logChooser;
 
     /**
@@ -69,6 +68,8 @@ public void autonomousInit() {
     @Override
     public void teleopInit() {
         Log.danger("Engaging teleoperated mode.");
+        // tell AutoManager not to preload or run any more programs
+        Core.getAutoManager().endPeriod();
         Core.getSubsystemManager().resetState();
     }
 
@@ -93,14 +94,6 @@ public void robotPeriodic() {
      */
     @Override
     public void disabledPeriodic() {
-        resetRobotState();
-    }
-
-    /**
-     * Utility functions to reset if auto has run when disabled.
-     */
-    private void resetRobotState() {
-        AutoFirstRun = true;
     }
 
     /**
@@ -109,10 +102,6 @@ private void resetRobotState() {
     @Override
     public void autonomousPeriodic() {
         update();
-
-        if (AutoFirstRun) {
-            AutoFirstRun = false;
-        }
     }
 
     /**

vendordeps/PathplannerLib.json

@@ -1,7 +1,7 @@
 {
     "fileName": "PathplannerLib.json",
     "name": "PathplannerLib",
-    "version": "2023.4.3",
+    "version": "2023.4.4",
     "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
     "mavenUrls": [
         "https://3015rangerrobotics.github.io/pathplannerlib/repo"
@@ -11,15 +11,15 @@
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-java",
-            "version": "2023.4.3"
+            "version": "2023.4.4"
         }
     ],
     "jniDependencies": [],
     "cppDependencies": [
         {
             "groupId": "com.pathplanner.lib",
             "artifactId": "PathplannerLib-cpp",
-            "version": "2023.4.3",
+            "version": "2023.4.4",
             "libName": "PathplannerLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,