diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/GuiAuto.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/GuiAuto.java index 267f5b9..9343a5e 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/GuiAuto.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/GuiAuto.java @@ -10,19 +10,75 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import java.io.File; import java.io.IOException; import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedList; import java.util.List; import java.util.concurrent.ExecutionException; -import static com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer.getCommandTranslator; +public class GuiAuto extends CommandBase { -public class GuiAuto implements Runnable { + + @Override + public void execute() { + if (isFirstRun) { + if (initialPose != null) { + //TODO: Set initial pose + AutonomousContainer.getInstance().printDebug("Set initial pose: " + initialPose); + } else { + AutonomousContainer.getInstance().printDebug("No initial pose set"); + } + + if (!abstractAutonomousSteps.isEmpty()) { + abstractAutonomousSteps.peek().initialize(); + } + + isFirstRun = false; + } + + if (abstractAutonomousSteps.isEmpty()) { + return; + } + + AbstractAutonomousStep autonomousStep = abstractAutonomousSteps.peek(); + + try { + if (autonomousStep.execute(scriptsToExecuteByTime, scriptsToExecuteByPercent)) { + autonomousStep.end(); + abstractAutonomousSteps.remove(); + if (!abstractAutonomousSteps.isEmpty()) { + abstractAutonomousSteps.peek().initialize(); + } + + //Sort the lists to make sure they are sorted by time + Collections.sort(scriptsToExecuteByTime); + Collections.sort(scriptsToExecuteByPercent); + } + } catch (CommandExecutionFailedException | ExecutionException e) { + DriverStation.reportError("Failed to execute autonomous step. " + e.getMessage(), e.getStackTrace()); + abstractAutonomousSteps.clear(); // Will end the auto + } + } + + @Override + public void end(boolean interrupted) { + if (!abstractAutonomousSteps.isEmpty()) { + abstractAutonomousSteps.peek().end(); + } + + abstractAutonomousSteps.clear(); + } + + @Override + public boolean isFinished() { + return abstractAutonomousSteps.isEmpty(); + } private static final Autonomous DO_NOTHING_AUTONOMOUS = new Autonomous(new ArrayList<>()); private @NotNull Autonomous autonomous = DO_NOTHING_AUTONOMOUS; // default to do nothing in case of some error @@ -37,7 +93,8 @@ public class GuiAuto implements Runnable { public GuiAuto(File autonomousFile) throws IOException { autonomous = (Autonomous) Serializer.deserializeFromFile(autonomousFile, Autonomous.class, autonomousFile.getName().endsWith(".json")); - init(); + this.setName(autonomousFile.getPath() + "GuiAuto"); + initialize(); } /** @@ -53,85 +110,35 @@ public GuiAuto(String autonomousJson) { DriverStation.reportError("Failed to deserialize auto. " + e.getMessage(), e.getStackTrace()); // The do nothing auto will be used } - init(); + this.setName("JsonGuiAuto"); + initialize(); } /** * Finds and saves the initial pose of the robot. */ - private void init() { + @Override + public void initialize() { for (AbstractAutonomousStep autonomousStep : autonomous.getAutonomousSteps()) { - if (autonomousStep instanceof TrajectoryAutonomousStep) { - TrajectoryAutonomousStep trajectoryAutonomousStep = (TrajectoryAutonomousStep) autonomousStep; + if (autonomousStep instanceof TrajectoryAutonomousStep trajectoryAutonomousStep) { Trajectory.State initialState = trajectoryAutonomousStep.getTrajectory().getStates().get(0); initialPose = new Pose2d(initialState.poseMeters.getTranslation(), trajectoryAutonomousStep.getRotations().get(0).getRotation()); break; } } - } - - /** - * Runs the autonomous. - */ - @Override - public void run() { - AutonomousContainer.getInstance().isInitialized(); - - Thread.currentThread().setUncaughtExceptionHandler((t, e) -> { - DriverStation.reportError("Uncaught exception in auto thread: " + e.getMessage(), e.getStackTrace()); - getCommandTranslator().stopRobot(); - }); - - if (autonomous == DO_NOTHING_AUTONOMOUS) { - DriverStation.reportError("No auto was loaded. Doing nothing.", false); - return; - } - - AutonomousContainer.getInstance().printDebug("Started Running: " + Timer.getFPGATimestamp()); - - - //Set our initial pose in our robot tracker - if (initialPose != null) { - getCommandTranslator().setRobotPose(initialPose); - AutonomousContainer.getInstance().printDebug("Set initial pose: " + initialPose); - } else { - AutonomousContainer.getInstance().printDebug("No initial pose set"); - } - - //Loop though all the steps and execute them - List scriptsToExecuteByTime = new ArrayList<>(); - List scriptsToExecuteByPercent = new ArrayList<>(); - - for (AbstractAutonomousStep autonomousStep : autonomous.getAutonomousSteps()) { - AutonomousContainer.getInstance().printDebug("Doing a step: " + Timer.getFPGATimestamp()); - if (Thread.interrupted()) { - getCommandTranslator().stopRobot(); - AutonomousContainer.getInstance().printDebug("Auto was interrupted " + Timer.getFPGATimestamp()); - return; - } + scriptsToExecuteByPercent = new ArrayList<>(); + scriptsToExecuteByTime = new ArrayList<>(); + abstractAutonomousSteps = new LinkedList<>(autonomous.getAutonomousSteps()); + isFirstRun = true; + } - try { - autonomousStep.execute(scriptsToExecuteByTime, scriptsToExecuteByPercent); - } catch (InterruptedException e) { - getCommandTranslator().stopRobot(); - AutonomousContainer.getInstance().printDebug("Auto prematurely stopped at " + Timer.getFPGATimestamp() + - ". This is not an error if you disabled your robot."); - if (AutonomousContainer.getInstance().areDebugPrintsEnabled()) { - e.printStackTrace(); - } - return; - } catch (CommandExecutionFailedException | ExecutionException e) { - getCommandTranslator().stopRobot(); - e.printStackTrace(); // We should always print this out since it is a fatal error - return; - } - } + private List scriptsToExecuteByTime; + private List scriptsToExecuteByPercent; + private LinkedList abstractAutonomousSteps; - System.out.println("Finished Autonomous at " + Timer.getFPGATimestamp()); - getCommandTranslator().stopRobot(); - } + private boolean isFirstRun = true; /** * Gets the initial pose of the robot. diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/annotations/AutoBuilderRobotSide.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/annotations/AutoBuilderRobotSide.java new file mode 100644 index 0000000..da58f84 --- /dev/null +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/annotations/AutoBuilderRobotSide.java @@ -0,0 +1,14 @@ +package com.dacubeking.AutoBuilder.robot.annotations; + +import java.lang.annotation.ElementType; +import java.lang.annotation.Retention; +import java.lang.annotation.RetentionPolicy; +import java.lang.annotation.Target; + +/** + * Annotation that marks that this field/method is only used when running on the robot + */ +@Retention(RetentionPolicy.SOURCE) +@Target({ElementType.METHOD, ElementType.FIELD, ElementType.TYPE, ElementType.CONSTRUCTOR, ElementType.PARAMETER}) +public @interface AutoBuilderRobotSide { +} diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/command/AutoBuilderSwerveControllerCommand.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/command/AutoBuilderSwerveControllerCommand.java new file mode 100644 index 0000000..b5ac7fc --- /dev/null +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/command/AutoBuilderSwerveControllerCommand.java @@ -0,0 +1,105 @@ +package com.dacubeking.AutoBuilder.robot.command; + +import com.dacubeking.AutoBuilder.robot.robotinterface.TrajectoryBuilderInfo; +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; + +import java.util.function.Consumer; +import java.util.function.Supplier; + +import static edu.wpi.first.util.ErrorMessages.requireNonNullParam; + +/** + * {@link SwerveControllerCommand}, but adds a check for position error before finishing a path, and removes constructors the + * imply the heading from the trajectory. (as the {@link TrajectoryBuilderInfo#desiredHeadingSupplier()} should be passed to the + * desiredRotation + */ +public class AutoBuilderSwerveControllerCommand extends SwerveControllerCommand { + /** + * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not + * return output voltages but rather raw module states from the position controllers which need to be put into a velocity + * PID. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path. + * This is left to the user to do since it is not appropriate for paths with nonstationary endstates. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param xController The Trajectory Tracker PID controller for the robot's x position. + * @param yController The Trajectory Tracker PID controller for the robot's y position. + * @param thetaController The Trajectory Tracker PID controller for angle for the robot. + * @param tolerance tolerance for the HolonomicDriveController. Will prevent the path from completing if the robot is + * not within the defined tolerance. see: {@link HolonomicDriveController#setTolerance(Pose2d)} + * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each time step. + * @param outputModuleStates The raw output module states from the position controllers. + * @param requirements The subsystems to require. + */ + public AutoBuilderSwerveControllerCommand(Trajectory trajectory, + Supplier pose, + SwerveDriveKinematics kinematics, + PIDController xController, + PIDController yController, + ProfiledPIDController thetaController, + Pose2d tolerance, + Supplier desiredRotation, + Consumer outputModuleStates, + Subsystem... requirements) { + + this( + trajectory, + pose, + kinematics, + new HolonomicDriveController( + requireNonNullParam(xController, "xController", "SwerveControllerCommand"), + requireNonNullParam(yController, "yController", "SwerveControllerCommand"), + requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand")), + tolerance, + desiredRotation, + outputModuleStates, + requirements); + } + + /** + * Constructs a new SwerveControllerCommand that when executed will follow the provided trajectory. This command will not + * return output voltages but rather raw module states from the position controllers which need to be put into a velocity + * PID. + * + *

Note: The controllers will *not* set the outputVolts to zero upon completion of the path- + * this is left to the user, since it is not appropriate for paths with nonstationary endstates. + * + * @param trajectory The trajectory to follow. + * @param pose A function that supplies the robot pose - use one of the odometry classes to provide this. + * @param kinematics The kinematics for the robot drivetrain. + * @param controller The HolonomicDriveController for the drivetrain. + * @param tolerance tolerance for the HolonomicDriveController. Will prevent the path from completing if the robot is + * not within the defined tolerance. see: {@link HolonomicDriveController#setTolerance(Pose2d)} + * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each time step. + * @param outputModuleStates The raw output module states from the position controllers. + * @param requirements The subsystems to require. + */ + public AutoBuilderSwerveControllerCommand(Trajectory trajectory, Supplier pose, SwerveDriveKinematics kinematics, + HolonomicDriveController controller, + Pose2d tolerance, Supplier desiredRotation, + Consumer outputModuleStates, Subsystem... requirements) { + super(trajectory, pose, kinematics, controller, desiredRotation, outputModuleStates, requirements); + this.controller = controller; + controller.setTolerance(tolerance); + } + + private final HolonomicDriveController controller; + + @Override + public boolean isFinished() { + assert controller != null; + return controller.atReference(); + } +} diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/AutonomousContainer.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/AutonomousContainer.java index 533bfe6..9cec6bd 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/AutonomousContainer.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/AutonomousContainer.java @@ -4,23 +4,23 @@ import com.dacubeking.AutoBuilder.robot.GuiAuto; import com.dacubeking.AutoBuilder.robot.NetworkAuto; import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderAccessible; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderRobotSide; import com.dacubeking.AutoBuilder.robot.annotations.RequireWait; import com.google.common.base.Preconditions; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableEvent.Kind; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; import org.jetbrains.annotations.ApiStatus.Internal; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import java.io.File; import java.io.IOException; -import java.lang.Thread.State; import java.lang.reflect.InvocationTargetException; import java.util.*; import java.util.concurrent.CompletableFuture; @@ -30,8 +30,11 @@ import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; +import java.util.function.Function; import java.util.regex.Pattern; +@AutoBuilderRobotSide public final class AutonomousContainer { private final @NotNull NetworkTableInstance instance = NetworkTableInstance.getDefault(); @@ -43,8 +46,8 @@ public final class AutonomousContainer { private @Nullable NetworkAuto networkAuto = null; private final @NotNull ExecutorService deserializerExecutor = Executors.newSingleThreadExecutor(); - private @Nullable Thread autoThread = null; - private final @NotNull Object autoThreadLock = new Object(); + private Function trajectoryFollowerSupplier; + private Consumer initialPoseSetter; private static final AutonomousContainer autonomousContainer = new AutonomousContainer(); @@ -56,7 +59,6 @@ private AutonomousContainer() { } - private CommandTranslator commandTranslator; private boolean isHolonomic; private volatile boolean debugPrints = false; @@ -67,38 +69,31 @@ private AutonomousContainer() { private static final String AUTO_DIRECTORY = Filesystem.getDeployDirectory().getAbsoluteFile() + "/autos/"; /** - * @param isHolonomic Is the robot using a holonomic drivetrain? (ex: swerve or mecanum) - * @param commandTranslator The command translator to use - * @param crashOnError Should the robot crash on error? If this is enabled, and an auto fails to load, the robot will - * crash. If this is disabled, the robot will skip the invalid auto and continue to the next one. - * @param parentObjects Objects that can be used to access other objects annotated with {@link AutoBuilderAccessible}. - * @param timedRobot The timed robot to use to create the period function for the autos. This can be null if you're - * running autos completely asynchronously. + * @param isHolonomic Is the robot using a holonomic drivetrain? (ex: swerve or mecanum) + * @param trajectoryFollowerSupplier A supplier that returns a command that will follow the trajectory from a + * {@link TrajectoryBuilderInfo} object. + * @param initialPoseSetter A consumer that sets the initial pose of the robot. + * @param parentObjects Objects that can be used to access other objects annotated with + * {@link AutoBuilderAccessible}. + * @throws NullPointerException If either {@code trajectoryFollowerSupplier} or {@code initialPoseSetter} is null. */ @SuppressWarnings("unused") public synchronized void initialize( boolean isHolonomic, - @NotNull CommandTranslator commandTranslator, - boolean crashOnError, - @Nullable TimedRobot timedRobot, + @NotNull Function trajectoryFollowerSupplier, + @NotNull Consumer initialPoseSetter, Object... parentObjects ) { - Preconditions.checkArgument(this.commandTranslator == null, "The Autonomous Container has already been initialized"); - Preconditions.checkArgument(commandTranslator != null, "Command translator cannot be null"); - Preconditions.checkArgument(!isHolonomic || commandTranslator.setAutonomousRotation != null, - "The command translator must have a setAutonomousRotation method if the robot is holonomic"); - - if (commandTranslator.runOnMainThread) { - Preconditions.checkArgument(timedRobot != null, - "TimedRobot cannot be null if autonomous commands are being issues on the main thread"); - timedRobot.addPeriodic(this::onAutoPeriodic, 0.001); - } + Preconditions.checkNotNull(trajectoryFollowerSupplier, "Trajectory follower supplier cannot be null"); + Preconditions.checkNotNull(initialPoseSetter, "Initial pose setter cannot be null"); + - initializeAccessibleInstances(parentObjects, crashOnError); + initializeAccessibleInstances(parentObjects, true); printDebug("Initialized Accessible Instances"); + //Create the listener for network autos NetworkTableInstance.getDefault() .addListener(autoPath, EnumSet.of(Kind.kValueRemote, Kind.kImmediate, Kind.kProperties), event -> { @@ -122,11 +117,12 @@ public synchronized void initialize( }); this.isHolonomic = isHolonomic; - this.commandTranslator = commandTranslator; + this.trajectoryFollowerSupplier = trajectoryFollowerSupplier; + this.initialPoseSetter = initialPoseSetter; long startLoadingTime = System.currentTimeMillis(); - findAutosAndLoadAutos(new File(AUTO_DIRECTORY), crashOnError); + findAutosAndLoadAutos(new File(AUTO_DIRECTORY)); System.out.println("Found " + numAutonomousFiles + " Autos. Waiting for them to load"); blockedThread = Thread.currentThread(); @@ -139,11 +135,6 @@ public synchronized void initialize( } } - if (crashOnError) { - Preconditions.checkState(loadedAutosCount.get() == numAutonomousFiles, - "Not all autonomous files were successfully loaded"); - } - System.out.println("Successfully loaded " + autonomousList.size() + " auto" + (autonomousList.size() == 1 ? "" : "s") + " with " + (loadedAutosCount.get() - successfullyLoadedAutosCount.get()) + @@ -251,18 +242,16 @@ private synchronized void initializeAccessibleInstances(Object[] parentObjects, /** * Recursively finds all autonomous files in the given directory and loads them. * - * @param directory The directory to search in. - * @param crashOnError Should the robot crash on error? If this is enabled, and an auto fails to load, the robot will crash. - * If this is disabled, the robot will skip the invalid auto and continue to the next one. + * @param directory The directory to search in. */ - private synchronized void findAutosAndLoadAutos(File directory, boolean crashOnError) { + private synchronized void findAutosAndLoadAutos(File directory) { File[] autos = directory.listFiles(); if (autos == null) { System.out.println("No autos files found"); } else { for (File file : autos) { if (file.isDirectory()) { - findAutosAndLoadAutos(file, crashOnError); + findAutosAndLoadAutos(file); continue; } @@ -271,9 +260,6 @@ private synchronized void findAutosAndLoadAutos(File directory, boolean crashOnE if (fileName.endsWith(".json") || fileName.endsWith(".auto")) { if (file.getName().contains("NOTDEPLOYABLE")) { System.out.println("Skipping " + file.getAbsolutePath() + " because it is marked as NOTDEPLOYABLE"); - if (crashOnError) { - throw new RuntimeException("An un-deployable file was found"); - } continue; } printDebug("Found auto file: " + file.getAbsolutePath()); @@ -307,13 +293,9 @@ private void incrementLoadedAutos() { } } - public synchronized static CommandTranslator getCommandTranslator() { - return getInstance().commandTranslator; - } - @Internal public synchronized void isInitialized() { - Preconditions.checkArgument(commandTranslator != null, "The Autonomous Container must be initialized before any " + + Preconditions.checkNotNull(initialPoseSetter, "The Autonomous Container must be initialized before any " + "autonomous can be run. (Call initialize(...) first)"); } @@ -335,12 +317,12 @@ public void printDebug(String message) { } @Internal - public @NotNull Hashtable getAccessibleInstances() { + public @NotNull Map getAccessibleInstances() { return parentObjects; } @Internal - public List getRequireWaitObjects() { + public @NotNull List getRequireWaitObjects() { return requireWaitObjects; } @@ -349,12 +331,22 @@ public void setDebugPrints(boolean debugPrints) { this.debugPrints = debugPrints; } + @Internal + public @NotNull Function getTrajectoryFollowerSupplier() { + return trajectoryFollowerSupplier; + } + + @Internal + public @NotNull Consumer getInitialPoseSetter() { + return initialPoseSetter; + } + /** * @return A list of the names of all the autos that have been loaded. The name is the name of the file, without the .json */ @SuppressWarnings("unused") - public ArrayList getAutonomousNames() { + public List getAutonomousNames() { ArrayList names = new ArrayList<>(autonomousList.size()); for (File absoluteFilePath : autonomousList.keySet()) { String fileName = absoluteFilePath.getName(); @@ -437,8 +429,7 @@ public synchronized void runAutonomous(String name, String side, boolean allowRu return; } - // Ensure that no other autos are currently running - runAuto(selectedAuto); + selectedAuto.schedule(); } /** @@ -467,64 +458,6 @@ public synchronized void runAutonomous(File file, boolean allowRunningNetworkAut DriverStation.reportError("Could not find auto: " + file.getAbsolutePath(), false); return; } - Thread.interrupted(); // Clear the interrupted flag on the calling thread - runAuto(selectedAuto); - } - - private void runAuto(@NotNull GuiAuto selectedAuto) { - // Ensure that no other autos are currently running - killAuto(); - commandTranslator.clearCommandQueue(); - - // We then create a new thread to run the auto and run it - synchronized (autoThreadLock) { - autoThread = new Thread(selectedAuto); - autoThread.start(); - } - } - - /** - * Kills the currently running autonomous. - */ - public void killAuto() { - synchronized (autoThreadLock) { - if (autoThread != null && autoThread.isAlive()) { - autoThread.interrupt(); - - double nextStackTracePrint = Timer.getFPGATimestamp() + 1; - while (autoThread.isAlive() && autoThread.getState() != State.TERMINATED) { - if (Timer.getFPGATimestamp() > nextStackTracePrint) { - Exception throwable = new Exception( - "Waiting for auto to die. autoThread.getState() = " + autoThread.getState() + - "\n Take a look at the stack trace for the auto thread bellow. Ensure that your auto " + - "will " + - "exit when it is interrupted."); - throwable.setStackTrace(autoThread.getStackTrace()); - throwable.printStackTrace(); - if (commandTranslator.runOnMainThread) { - Exception throwable2 = new Exception("The auto is running on the main thread. " + - "Double check the stack trace below and ensure that nothing is blocking your main thread." + - "(Note: The below stack trace will be the stack trace of the main thread, if you called " + - "killAuto() from the main thread"); - throwable2.fillInStackTrace(); - throwable2.printStackTrace(); - } - nextStackTracePrint = Timer.getFPGATimestamp() + 5; - } - - try { - //noinspection BusyWait - Thread.sleep(10); - } catch (InterruptedException e) { - e.printStackTrace(); - } - } - getCommandTranslator().stopRobot(); - } - } - } - - private void onAutoPeriodic() { - commandTranslator.onPeriodic(); + selectedAuto.schedule(); } } diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/CommandTranslator.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/CommandTranslator.java deleted file mode 100644 index 7bd5221..0000000 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/CommandTranslator.java +++ /dev/null @@ -1,180 +0,0 @@ -package com.dacubeking.AutoBuilder.robot.robotinterface; - -import com.google.common.base.Preconditions; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.wpilibj.Timer; -import org.jetbrains.annotations.ApiStatus.Internal; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -import java.util.concurrent.CompletableFuture; -import java.util.concurrent.ConcurrentLinkedQueue; -import java.util.concurrent.ExecutionException; -import java.util.function.BooleanSupplier; -import java.util.function.Consumer; -import java.util.function.DoubleSupplier; - -public class CommandTranslator { - protected final @NotNull Consumer setNewTrajectory; - protected final @NotNull Runnable stopRobot; - protected final @Nullable Consumer setAutonomousRotation; - private final @NotNull BooleanSupplier isTrajectoryDone; - private final @NotNull DoubleSupplier getTrajectoryElapsedTime; - private final Consumer setRobotPose; - @Internal public final boolean runOnMainThread; - - private volatile Rotation2d wantedRotation = new Rotation2d(); - /** - * Used to store the time that the trajectory started. This is used to calculate the elapsed time of the trajectory if the - * getTrajectoryElapsedTime lambda is null. - */ - private double trajectoryStartTime = 0.0; - - /** - * @param setNewTrajectory The consumer to call to set the new trajectory - * @param stopRobot The runnable to call to stop the robot - * @param setAutonomousRotation The consumer to call to set the autonomous rotation (this can be null is the robot is not - * holonomic or if you plan to poll {@link #getWantedRotation()} instead) - * @param isTrajectoryDone The boolean supplier to call to check if the trajectory is done. This lambada should return - * false until the path has been fully (and is within error of the final position/rotation) - * driven. - * @param getTrajectoryElapsedTime The double supplier to call to get the elapsed time of the trajectory. This lambada must - * return 0.0 immediately after a new trajectory is set and should return the elapsed time of - * the current trajectory that is being driven. (If this is null, the elapsed time will start - * as soon as the trajectory is set) - * @param setRobotPose The consumer to call to set the initial pose of the robot at the start of autonomous - * @param runOnMainThread Whether to run the commands on the main thread. If this is ture, the commands will be run - * on the main thread. If this is false, the commands will be run on the autonomous thread. If - * you are unsure, it is safer to leave this as true. If you've designed your robot code to be - * thread safe, you can set this to false. It will allow the methods you call to be blocking - * which can simplify some code. - */ - public CommandTranslator( - @NotNull Consumer setNewTrajectory, - @NotNull Runnable stopRobot, - @Nullable Consumer setAutonomousRotation, - @NotNull BooleanSupplier isTrajectoryDone, - @Nullable DoubleSupplier getTrajectoryElapsedTime, - @NotNull Consumer setRobotPose, - boolean runOnMainThread - ) { - Preconditions.checkArgument(setNewTrajectory != null, "setNewTrajectory cannot be null"); - Preconditions.checkArgument(stopRobot != null, "stopRobot cannot be null"); - Preconditions.checkArgument(isTrajectoryDone != null, "isTrajectoryDone cannot be null"); - Preconditions.checkArgument(setRobotPose != null, "setRobotPose cannot be null"); - - if (getTrajectoryElapsedTime == null) { - this.getTrajectoryElapsedTime = () -> Timer.getFPGATimestamp() - trajectoryStartTime; - this.setNewTrajectory = setNewTrajectory.andThen((trajectory) -> trajectoryStartTime = Timer.getFPGATimestamp()); - } else { - this.getTrajectoryElapsedTime = getTrajectoryElapsedTime; - this.setNewTrajectory = setNewTrajectory; - } - if (setAutonomousRotation == null) { - this.setAutonomousRotation = (rotation2d) -> wantedRotation = rotation2d; - } else { - this.setAutonomousRotation = setAutonomousRotation.andThen((rotation2d) -> wantedRotation = rotation2d); - } - - this.stopRobot = stopRobot; - this.isTrajectoryDone = isTrajectoryDone; - this.setRobotPose = setRobotPose; - this.runOnMainThread = runOnMainThread; - } - - private final @NotNull ConcurrentLinkedQueue commandQueue = new ConcurrentLinkedQueue<>(); - - @Internal - public void setNewTrajectory(@NotNull Trajectory trajectory) { - if (runOnMainThread) { - commandQueue.add(() -> setNewTrajectory.accept(trajectory)); - } else { - setNewTrajectory.accept(trajectory); - } - } - - @Internal - public void stopRobot() { - if (runOnMainThread) { - commandQueue.add(stopRobot); - } else { - stopRobot.run(); - } - } - - @Internal - public void setAutonomousRotation(@NotNull Rotation2d rotation) { - if (AutonomousContainer.getInstance().isHolonomic()) { - assert setAutonomousRotation != null; - if (runOnMainThread) { - commandQueue.add(() -> setAutonomousRotation.accept(rotation)); - } else { - setAutonomousRotation.accept(rotation); - } - } - } - - @Internal - protected void clearCommandQueue() { - commandQueue.clear(); - } - - @Internal - public void setRobotPose(@NotNull Pose2d pose) { - if (runOnMainThread) { - commandQueue.add(() -> setRobotPose.accept(pose)); - } else { - setRobotPose.accept(pose); - } - } - - @Internal - public boolean isTrajectoryDone() throws ExecutionException, InterruptedException { - if (runOnMainThread) { - CompletableFuture future = new CompletableFuture<>(); - commandQueue.add(() -> future.complete(isTrajectoryDone.getAsBoolean())); - return future.get(); - } else { - return isTrajectoryDone.getAsBoolean(); - } - } - - @Internal - public double getTrajectoryElapsedTime() throws ExecutionException, InterruptedException { - if (runOnMainThread) { - CompletableFuture future = new CompletableFuture<>(); - commandQueue.add(() -> future.complete(getTrajectoryElapsedTime.getAsDouble())); - return future.get(); - } else { - return getTrajectoryElapsedTime.getAsDouble(); - } - } - - @Internal - public void runOnMainThread(@NotNull Runnable runnable) { - commandQueue.add(runnable); - } - - /** - * Runs commands on the main thread. (called every 1ms) - */ - @Internal - protected void onPeriodic() { - while (!commandQueue.isEmpty()) { - @Nullable Runnable commandToRun = commandQueue.poll(); - if (commandToRun != null) { - commandToRun.run(); - } - } - } - - /** - * Gets the wanted rotation of the robot. This is the current goal rotation of the robot. This is only used if the robot is - * holonomic. - */ - public @NotNull Rotation2d getWantedRotation() { - return wantedRotation; - } -} diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/TrajectoryBuilderInfo.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/TrajectoryBuilderInfo.java new file mode 100644 index 0000000..b76d3c4 --- /dev/null +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/robotinterface/TrajectoryBuilderInfo.java @@ -0,0 +1,30 @@ +package com.dacubeking.AutoBuilder.robot.robotinterface; + +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderRobotSide; +import com.dacubeking.AutoBuilder.robot.command.AutoBuilderSwerveControllerCommand; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; +import org.jetbrains.annotations.NotNull; + +import java.util.function.Supplier; + +/** + * Stores the trajectory and the target pose supplier that should be used to follow the trajectory. + *

+ * Important Considerations: + *

    + *
  • Your code needs to stop the robot from moving when this command finishes/is canceled. Setting the default + * command for your drivetrain to use your controller inputs w/ brake mode enabled should achieve this.
    • + *
  • Consider using {@link AutoBuilderSwerveControllerCommand} instead of {@link SwerveControllerCommand}
    • + *
+ * + * @param trajectory The trajectory to follow + * @param desiredHeadingSupplier A supplier that returns the target pose for the trajectory follower. If you're using a + * non-holonomic drivetrain, you can ignore this. + */ +@AutoBuilderRobotSide +public record TrajectoryBuilderInfo( + @NotNull Trajectory trajectory, + @NotNull Supplier desiredHeadingSupplier +) {} diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/AbstractAutonomousStep.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/AbstractAutonomousStep.java index 8346923..98a0824 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/AbstractAutonomousStep.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/AbstractAutonomousStep.java @@ -1,5 +1,6 @@ package com.dacubeking.AutoBuilder.robot.serialization; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderRobotSide; import com.dacubeking.AutoBuilder.robot.serialization.command.CommandExecutionFailedException; import com.dacubeking.AutoBuilder.robot.serialization.command.SendableScript; import com.fasterxml.jackson.annotation.*; @@ -38,13 +39,27 @@ protected AbstractAutonomousStep(@JsonProperty(required = true, value = "closed" * * @param scriptsToExecuteByTime A mutable arraylist representing scripts to run while driving the autonomous path. * @param scriptsToExecuteByPercent A mutable arraylist representing scripts to run while driving the autonomous path. - * @throws InterruptedException Thrown if the thread is interrupted (ex: auto is killed). + * @return true if the autonomous step is finished, false otherwise * @throws CommandExecutionFailedException Thrown if a script fails to execute. * @throws ExecutionException Thrown if something goes wrong running a command on the main thread. */ - public abstract void execute(List scriptsToExecuteByTime, - List scriptsToExecuteByPercent) throws InterruptedException, - CommandExecutionFailedException, ExecutionException; + @AutoBuilderRobotSide + public abstract boolean execute(List scriptsToExecuteByTime, + List scriptsToExecuteByPercent) throws CommandExecutionFailedException, + ExecutionException; + + /** + * End this autonomous step. + */ + @AutoBuilderRobotSide + public abstract void end(); + + /** + * Initialize this autonomous step. + */ + @AutoBuilderRobotSide + public abstract void initialize(); + @JsonProperty("closed") public boolean isClosed() { diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/ScriptAutonomousStep.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/ScriptAutonomousStep.java index 1025a76..1f187a1 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/ScriptAutonomousStep.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/ScriptAutonomousStep.java @@ -40,23 +40,32 @@ public SendableScript getSendableScript() { * Runs the script */ @Override - public void execute(@NotNull List scriptsToExecuteByTime, - @NotNull List scriptsToExecuteByPercent) - throws InterruptedException, CommandExecutionFailedException, ExecutionException { + public boolean execute(@NotNull List scriptsToExecuteByTime, + @NotNull List scriptsToExecuteByPercent) + throws CommandExecutionFailedException, ExecutionException { if (sendableScript.getDelayType() == SendableScript.DelayType.TIME) { scriptsToExecuteByTime.add(sendableScript); - return; - } - - if (sendableScript.getDelayType() == SendableScript.DelayType.PERCENT) { + } else if (sendableScript.getDelayType() == SendableScript.DelayType.PERCENT) { scriptsToExecuteByPercent.add(sendableScript); - return; + } else { + return sendableScript.execute(); } - sendableScript.execute(); + return true; + } + + @Override + public void initialize() { + sendableScript.initialize(); + } + + @Override + public void end() { + } + @JsonProperty("script") public String getScript() { return script; diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TrajectoryAutonomousStep.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TrajectoryAutonomousStep.java index 1a0844b..3bc39ac 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TrajectoryAutonomousStep.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/TrajectoryAutonomousStep.java @@ -1,24 +1,27 @@ package com.dacubeking.AutoBuilder.robot.serialization; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderRobotSide; +import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer; +import com.dacubeking.AutoBuilder.robot.robotinterface.TrajectoryBuilderInfo; import com.dacubeking.AutoBuilder.robot.serialization.command.CommandExecutionFailedException; import com.dacubeking.AutoBuilder.robot.serialization.command.SendableScript; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.spline.Spline.ControlVector; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; import org.jetbrains.annotations.ApiStatus.Internal; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; -import java.util.Collections; import java.util.List; import java.util.concurrent.ExecutionException; -import static com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer.getCommandTranslator; @JsonIgnoreProperties(ignoreUnknown = true) @Internal @@ -77,68 +80,111 @@ public Trajectory getTrajectory() { return trajectory; } + @Override + @AutoBuilderRobotSide + public void end() { + pathDriveCommand.cancel(); + pathDriveCommand = null; + } + + @Override + @AutoBuilderRobotSide + public void initialize() { + rotationIndex = 1; // Start at the second rotation (the first is the starting rotation) + isFirstRun = true; + } + + private int rotationIndex; + private boolean isFirstRun; + + private Timer timer = new Timer(); + private Command pathDriveCommand = null; + /** * Executes the trajectory * * @param scriptsToExecuteByTime An arraylist of the scripts to run during this path. * @param scriptsToExecuteByPercent An arraylist of the scripts to run during this path. - * @throws InterruptedException If the thread is interrupted (ex: the auto is killed). * @throws CommandExecutionFailedException If a script fails to execute. * @throws ExecutionException Something goes wrong running a command on the main thread. */ @Override - public void execute(@NotNull List scriptsToExecuteByTime, - @NotNull List scriptsToExecuteByPercent) - throws InterruptedException, CommandExecutionFailedException, ExecutionException { - //Sort the lists to make sure they are sorted by time - Collections.sort(scriptsToExecuteByTime); - Collections.sort(scriptsToExecuteByPercent); - - if (rotations.size() > 0) { - //Scripts for non-holonomic won't have any rotations (since the rotation is based on the driven path) - getCommandTranslator().setAutonomousRotation(rotations.get(0).rotation); + @AutoBuilderRobotSide + public boolean execute(@NotNull List scriptsToExecuteByTime, + @NotNull List scriptsToExecuteByPercent) throws CommandExecutionFailedException, + ExecutionException { + + if (isFirstRun) { + if (!rotations.isEmpty()) { + //Scripts for non-holonomic won't have any rotations (since the rotation is based on the driven path) + targetRotation = rotations.get(0).rotation; + } + this.pathDriveCommand = AutonomousContainer.getInstance().getTrajectoryFollowerSupplier() + .apply(new TrajectoryBuilderInfo(trajectory, this::getTargetRotation)); + pathDriveCommand.schedule(); + timer.restart(); } - getCommandTranslator().setNewTrajectory(trajectory); //Send the auto to our drive class to be executed - int rotationIndex = 1; // Start at the second rotation (the first is the starting rotation) - while (!getCommandTranslator().isTrajectoryDone()) { // Wait till the auto is done - double startTime = Timer.getFPGATimestamp(); - final double elapsedTime = getCommandTranslator().getTrajectoryElapsedTime(); + final double elapsedTime = timer.get(); + assert pathDriveCommand != null; + if (pathDriveCommand.isFinished()) { + //Execute any remaining scripts + while (!scriptsToExecuteByTime.isEmpty()) { + // We have a script to execute, and it is time to execute it + if (scriptsToExecuteByTime.get(0).execute()) { + scriptsToExecuteByTime.remove(0); + } else { + break; // We've been told something hasn't finished executing. Continue & see if it's completed next time + } + } + + while (!scriptsToExecuteByPercent.isEmpty()) { + // We have a script to execute, and it is time to execute it + if (scriptsToExecuteByPercent.get(0).execute()) { + scriptsToExecuteByPercent.remove(0); + } else { + break; // We've been told something hasn't finished executing. Continue & see if it's completed next time + } + } + return scriptsToExecuteByPercent.isEmpty() && scriptsToExecuteByTime.isEmpty(); //If some commands are still + // executing we need to wait at this step until they're done. + } else { // Wait till the auto is done if (rotationIndex < rotations.size() && elapsedTime > rotations.get(rotationIndex).time) { // We've passed the time for the next rotation - getCommandTranslator().setAutonomousRotation(rotations.get(rotationIndex).rotation); //Set the rotation + targetRotation = rotations.get(rotationIndex).rotation; //Set the rotation rotationIndex++; // Increment the rotation index } - if (!scriptsToExecuteByTime.isEmpty() + while (!scriptsToExecuteByTime.isEmpty() && scriptsToExecuteByTime.get(0).getDelay() <= elapsedTime) { // We have a script to execute, and it is time to execute it - scriptsToExecuteByTime.get(0).execute(); - scriptsToExecuteByTime.remove(0); + if (scriptsToExecuteByTime.get(0).execute()) { + scriptsToExecuteByTime.remove(0); + } else { + break; // We've been told something hasn't finished executing. Continue & see if it's completed next time + } } - if (!scriptsToExecuteByPercent.isEmpty() + while (!scriptsToExecuteByPercent.isEmpty() && scriptsToExecuteByPercent.get(0).getDelay() <= elapsedTime / trajectory.getTotalTimeSeconds()) { // We have a script to execute, and it is time to execute it - scriptsToExecuteByPercent.get(0).execute(); - scriptsToExecuteByPercent.remove(0); + if (scriptsToExecuteByPercent.get(0).execute()) { + scriptsToExecuteByPercent.remove(0); + } else { + break; // We've been told something hasn't finished executing. Continue & see if it's completed next time + } } - //noinspection BusyWait - Thread.sleep((long) (1000 * Math.max(startTime + PERIOD_TIME_S - Timer.getFPGATimestamp(), 0))); + return false; } - getCommandTranslator().stopRobot(); + } - //Execute any remaining scripts - for (SendableScript sendableScript : scriptsToExecuteByTime) { - sendableScript.execute(); - } - for (SendableScript sendableScript : scriptsToExecuteByPercent) { - sendableScript.execute(); - } - scriptsToExecuteByTime.clear(); - scriptsToExecuteByPercent.clear(); + private Rotation2d targetRotation = new Rotation2d(); + + @AutoBuilderRobotSide + public Rotation2d getTargetRotation() { + return targetRotation; } @Override diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java index cd49fc9..7179346 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableCommand.java @@ -1,15 +1,16 @@ package com.dacubeking.AutoBuilder.robot.serialization.command; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderRobotSide; import com.dacubeking.AutoBuilder.robot.annotations.RequireWait; import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer; import com.dacubeking.AutoBuilder.robot.utility.Utils; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; -import com.google.common.util.concurrent.UncheckedExecutionException; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.WaitCommand; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; @@ -20,16 +21,12 @@ import java.util.HashMap; import java.util.List; import java.util.Map; -import java.util.concurrent.CompletableFuture; import java.util.concurrent.ExecutionException; import java.util.function.Function; -import static com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer.getCommandTranslator; - @JsonIgnoreProperties(ignoreUnknown = true) public class SendableCommand { - public static final double LOOPING_PERIOD_SECONDS = 0.02; // 50 Hz/20 ms @JsonProperty("methodName") @NotNull protected final String methodName; @@ -40,10 +37,11 @@ public class SendableCommand { @JsonProperty("reflection") public final boolean reflection; - @JsonProperty("command") private final boolean command; + @JsonProperty("command") private boolean command; + @AutoBuilderRobotSide private boolean shouldWait; - + @AutoBuilderRobotSide private boolean shouldCancelCommand; private static final @NotNull Map> INFERABLE_TYPES_PARSER; @@ -110,6 +108,7 @@ public SendableCommand(@JsonProperty("methodName") @NotNull String methodName, } } + @AutoBuilderRobotSide private void init() { if (command) { // If we're a command, the command name is the method name @@ -214,11 +213,15 @@ private void init() { shouldWait = shouldWait || AutonomousContainer.getInstance().getRequireWaitObjects().contains(instance); - this.methodToCall = methodToCall; - this.instance = instance; - this.shouldWait = shouldWait; + if (!reflection && methodName.equals("sleep")) { + // We no longer implement sleep as an actual sleep so convert it to a wait command + command = true; + instance = new WaitCommand(Double.parseDouble(args[0])); + shouldWait = true; + } } + @AutoBuilderRobotSide private static void throwIllegalArgumentException(@NotNull String errorMessage, @Nullable Exception e) { DriverStation.reportError(errorMessage, false); throw new IllegalArgumentException(errorMessage, e); @@ -226,10 +229,12 @@ private static void throwIllegalArgumentException(@NotNull String errorMessage, @JsonIgnoreProperties @Nullable + @AutoBuilderRobotSide private Object instance; @JsonIgnoreProperties @Nullable + @AutoBuilderRobotSide private Method methodToCall; @JsonIgnoreProperties private final Object @NotNull [] objArgs; @@ -259,13 +264,24 @@ private static Class getPrimitiveClass(Class clazz) { } + @AutoBuilderRobotSide + public void setFirstRun() { + firstRun = true; + } + + + @AutoBuilderRobotSide + public boolean firstRun; + + /** - * @throws InterruptedException If the command fails do to an interrupt + * @return true if the command is finished, false otherwise * @throws CommandExecutionFailedException If the command fails to execute for any other reason * @throws ExecutionException Should never happen (for some reason the future was cancelled or threw and * exception) */ - protected void execute() throws InterruptedException, CommandExecutionFailedException, ExecutionException { + @AutoBuilderRobotSide + protected boolean execute() throws CommandExecutionFailedException, ExecutionException { if (!command && methodToCall == null && reflection) { throw new CommandExecutionFailedException("Method to call is null"); } @@ -274,87 +290,37 @@ protected void execute() throws InterruptedException, CommandExecutionFailedExce throw new CommandExecutionFailedException("Instance is null when calling a command"); } - // The built command's shouldn't be run on the main thread - if (getCommandTranslator().runOnMainThread && reflection) { - if (!command) { - while (true) { - double startTime = Timer.getFPGATimestamp(); // Time the command to keep the period constant - - CompletableFuture future = new CompletableFuture<>(); - - getCommandTranslator().runOnMainThread(() -> { - try { - future.complete(invokeMethod()); - } catch (CommandExecutionFailedException | InterruptedException e) { - future.complete(new UncheckedExecutionException(e)); - } - }); // Schedule the command to run on the main thread - - if (!shouldWait) { - return; - } - - Object result = future.get(); // Wait for the command to finish - - if (result instanceof UncheckedExecutionException) { // If the command failed rethrow the exception - Throwable exception = ((UncheckedExecutionException) result).getCause(); - if (exception instanceof InterruptedException) { - throw (InterruptedException) exception; - } else if (exception instanceof CommandExecutionFailedException) { - throw (CommandExecutionFailedException) exception; - } else { - throw new CommandExecutionFailedException( - "Unexpected Exception. Please report this: " + exception.getMessage(), exception); - } - } - - if (!(result instanceof Boolean) || result.equals(true)) { - break; // If the command returns true or is not a boolean, stop the command - } - - //Keep executing the method if it returns false - //noinspection BusyWait - Thread.sleep((long) Math.max((LOOPING_PERIOD_SECONDS - (Timer.getFPGATimestamp() - startTime)) * 1000, 0)); - } + if (command) { + var wpiCommand = (Command) instance; + if (shouldCancelCommand) { + wpiCommand.cancel(); } else { - if (shouldCancelCommand) { - getCommandTranslator().runOnMainThread(() -> ((Command) instance).cancel()); - } else { - getCommandTranslator().runOnMainThread(() -> ((Command) instance).schedule()); - - if (shouldWait) { - CompletableFuture future; - do { - //Wait for the command to finish - //noinspection BusyWait - Thread.sleep((long) LOOPING_PERIOD_SECONDS * 1000); - future = new CompletableFuture<>(); - final var finalFuture = future; - getCommandTranslator().runOnMainThread( - () -> finalFuture.complete(((Command) instance).isScheduled())); - } while (future.get()); - } + if (firstRun) { + wpiCommand.initialize(); + wpiCommand.schedule(); + CommandScheduler.getInstance().schedule(wpiCommand); + firstRun = false; + } + + if (shouldWait) { + return wpiCommand.isFinished(); } } } else { - if (command) { - throw new CommandExecutionFailedException("Commands must be run on the main thread"); + if (shouldCancelCommand) { + throw new CommandExecutionFailedException("Cannot cancel a method call"); } - while (true) { - double startTime = Timer.getFPGATimestamp(); - Object result = invokeMethod(); - if (shouldWait || !(result instanceof Boolean) || result.equals(true)) { - break; - } + var out = invokeMethod(); - //Keep executing the method if it returns false - //noinspection BusyWait - Thread.sleep((long) Math.max((LOOPING_PERIOD_SECONDS - (Timer.getFPGATimestamp() - startTime)) * 1000, 0)); + // If the method returns true & is a boolean, stop executing it. If + if (shouldWait && out instanceof Boolean) { + return (Boolean) out; } } + return true; } - private @Nullable Object invokeMethod() throws InterruptedException, CommandExecutionFailedException { + private @Nullable Object invokeMethod() throws CommandExecutionFailedException { assert !command; try { if (reflection) { @@ -363,12 +329,11 @@ protected void execute() throws InterruptedException, CommandExecutionFailedExce } else { switch (methodName) { case "print" -> System.out.println(objArgs[0]); - case "sleep" -> Thread.sleep((long) objArgs[0]); + case "sleep" -> throw new UnsupportedOperationException( + "The sleep method should have been converted to a command"); } return null; } - } catch (InterruptedException e) { - throw e; } catch (Exception e) { throw new CommandExecutionFailedException("Could not invoke method " + methodName + " due to: " + e.getMessage(), e); } diff --git a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableScript.java b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableScript.java index c7fe56f..958f0ce 100644 --- a/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableScript.java +++ b/autobuilder-robot/src/main/java/com/dacubeking/AutoBuilder/robot/serialization/command/SendableScript.java @@ -1,26 +1,45 @@ package com.dacubeking.AutoBuilder.robot.serialization.command; +import com.dacubeking.AutoBuilder.robot.annotations.AutoBuilderRobotSide; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonProperty; import org.jetbrains.annotations.NotNull; import java.util.ArrayList; +import java.util.LinkedList; import java.util.concurrent.ExecutionException; public class SendableScript implements Comparable { + @JsonIgnore + @AutoBuilderRobotSide + public LinkedList executionQueue; + /** - * @throws InterruptedException if the thread is interrupted while executing the commands + * @return true if all the commands have finished executing, false if some commands still need to be executed. */ - public void execute() throws InterruptedException, CommandExecutionFailedException, ExecutionException { - for (SendableCommand command : commands) { - if (Thread.interrupted()) { - throw new InterruptedException("Interrupted while trying to execute a script "); - } - command.execute(); + @AutoBuilderRobotSide + public boolean execute() throws CommandExecutionFailedException, ExecutionException { + if (executionQueue.isEmpty()) { + return true; } + + SendableCommand command = executionQueue.peek(); + + + // Keep executing commands until we reach one we need to wait at. + while (command.execute()) { + executionQueue.remove(); + } + + return executionQueue.isEmpty(); + } + + public void initialize() { + commands.forEach(SendableCommand::setFirstRun); + executionQueue = new LinkedList<>(commands); } @Override