diff --git a/build.gradle b/build.gradle index ecad8018..aec7d2ec 100644 --- a/build.gradle +++ b/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 diff --git a/src/main/java/org/wildstang/framework/auto/AutoManager.java b/src/main/java/org/wildstang/framework/auto/AutoManager.java index 31284155..365f5064 100644 --- a/src/main/java/org/wildstang/framework/auto/AutoManager.java +++ b/src/main/java/org/wildstang/framework/auto/AutoManager.java @@ -23,6 +23,8 @@ public class AutoManager { private boolean programFinished; private SendableChooser chooser; private SendableChooser 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,7 +138,6 @@ private void startProgram(AutoProgram program) { runningProgram = program; program.initialize(); Log.info("Starting auto program: " + program.toString()); - SmartDashboard.putString("Running Autonomous Program", program.toString()); } /** @@ -88,6 +145,8 @@ private void startProgram(AutoProgram program) { */ 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; + } } diff --git a/src/main/java/org/wildstang/framework/auto/AutoProgram.java b/src/main/java/org/wildstang/framework/auto/AutoProgram.java index 185783fb..97935cdd 100644 --- a/src/main/java/org/wildstang/framework/auto/AutoProgram.java +++ b/src/main/java/org/wildstang/framework/auto/AutoProgram.java @@ -17,7 +17,7 @@ public abstract class AutoProgram { protected final List 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(); diff --git a/src/main/java/org/wildstang/hardware/roborio/outputs/WsSparkMax.java b/src/main/java/org/wildstang/hardware/roborio/outputs/WsSparkMax.java index edc98002..d7171955 100644 --- a/src/main/java/org/wildstang/hardware/roborio/outputs/WsSparkMax.java +++ b/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 diff --git a/src/main/java/org/wildstang/year2023/robot/Robot.java b/src/main/java/org/wildstang/year2023/robot/Robot.java index 6d1dc510..9c285b2f 100644 --- a/src/main/java/org/wildstang/year2023/robot/Robot.java +++ b/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 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; - } } /** diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index fab5e5b8..9e913435 100644 --- a/vendordeps/PathplannerLib.json +++ b/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,7 +11,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.3" + "version": "2023.4.4" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.3", + "version": "2023.4.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 55bc0990..614dc3ab 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix (v5)", - "version": "5.30.4", + "version": "5.31.0+23.2.2", "frcYear": 2023, "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ @@ -12,19 +12,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.30.4" + "version": "5.31.0" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.30.4" + "version": "5.31.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.4", + "version": "5.31.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.4", + "version": "5.31.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -48,9 +48,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -61,9 +61,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -74,9 +74,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -87,9 +87,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -100,9 +100,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -113,9 +113,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -126,9 +126,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -139,9 +139,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,9 +152,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -165,9 +165,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.5", + "version": "23.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -182,7 +182,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -197,7 +197,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": true, @@ -212,7 +212,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": true, @@ -225,9 +225,9 @@ "simMode": "hwsim" }, { - "groupId": "com.ctre.phoenixpro", + "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "wpiapi-cpp-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_Phoenix_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -257,7 +257,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "api-cpp-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixSim", "headerClassifier": "headers", "sharedLibrary": true, @@ -272,7 +272,7 @@ { "groupId": "com.ctre.phoenix.sim", "artifactId": "cci-sim", - "version": "5.30.4", + "version": "5.31.0", "libName": "CTRE_PhoenixCCISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -285,9 +285,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -300,9 +300,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -315,9 +315,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -330,9 +330,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -345,9 +345,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -360,9 +360,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -375,9 +375,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -390,9 +390,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -405,9 +405,9 @@ "simMode": "swsim" }, { - "groupId": "com.ctre.phoenixpro.sim", + "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.0.5", + "version": "23.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,