diff --git a/wpilibj/build.gradle b/wpilibj/build.gradle index 3e5bbca8e0..83a6e55dde 100644 --- a/wpilibj/build.gradle +++ b/wpilibj/build.gradle @@ -55,6 +55,10 @@ compileJava { dependsOn generateJavaVersion } +repositories { + jcenter() +} + dependencies { compile project(':hal') compile project(':wpiutil') @@ -62,6 +66,7 @@ dependencies { compile project(':cscore') compile project(':cameraserver') testCompile 'com.google.guava:guava:19.0' + testCompile 'org.mockito:mockito-core:2.27.0' devCompile project(':hal') devCompile project(':wpiutil') devCompile project(':ntcore') diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java new file mode 100644 index 0000000000..23410272c8 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Command.java @@ -0,0 +1,304 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Set; +import java.util.function.BooleanSupplier; + +/** + * A state machine representing a complete action to be performed by the robot. Commands are + * run by the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to + * build complicated multi-step actions without the need to roll the state machine logic themselves. + * + *

Commands are run synchronously from the main robot loop; no multithreading is used, unless + * specified explicitly from the command implementation. + */ +@SuppressWarnings("PMD.TooManyMethods") +public interface Command { + + /** + * The initial subroutine of a command. Called once when the command is initially scheduled. + */ + default void initialize() { + } + + /** + * The main body of a command. Called repeatedly while the command is scheduled. + */ + default void execute() { + } + + /** + * The action to take when the command ends. Called when either the command finishes normally, + * or when it interrupted/canceled. + * + * @param interrupted whether the command was interrupted/canceled + */ + default void end(boolean interrupted) { + } + + /** + * Whether the command has finished. Once a command finishes, the scheduler will call its + * end() method and un-schedule it. + * + * @return whether the command has finished. + */ + default boolean isFinished() { + return false; + } + + /** + * Specifies the set of subsystems used by this command. Two commands cannot use the same + * subsystem at the same time. If the command is scheduled as interruptible and another + * command is scheduled that shares a requirement, the command will be interrupted. Else, + * the command will not be scheduled. If no subsystems are required, return an empty set. + * + *

Note: it is recommended that user implementations contain the requirements as a field, + * and return that field here, rather than allocating a new set every time this is called. + * + * @return the set of subsystems that are required + */ + Set getRequirements(); + + /** + * Decorates this command with a timeout. If the specified timeout is exceeded before the command + * finishes normally, the command will be interrupted and un-scheduled. Note that the + * timeout only applies to the command returned by this method; the calling command is + * not itself changed. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param seconds the timeout duration + * @return the command with the timeout added + */ + default Command withTimeout(double seconds) { + return new ParallelRaceGroup(this, new WaitCommand(seconds)); + } + + /** + * Decorates this command with an interrupt condition. If the specified condition becomes true + * before the command finishes normally, the command will be interrupted and un-scheduled. + * Note that this only applies to the command returned by this method; the calling command + * is not itself changed. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param condition the interrupt condition + * @return the command with the interrupt condition added + */ + default Command interruptOn(BooleanSupplier condition) { + return new ParallelRaceGroup(this, new WaitUntilCommand(condition)); + } + + /** + * Decorates this command with a runnable to run after the command finishes. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param toRun the Runnable to run + * @return the decorated command + */ + default Command whenFinished(Runnable toRun) { + return new SequentialCommandGroup(this, new InstantCommand(toRun)); + } + + /** + * Decorates this command with a runnable to run before this command starts. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param toRun the Runnable to run + * @return the decorated command + */ + default Command beforeStarting(Runnable toRun) { + return new SequentialCommandGroup(new InstantCommand(toRun), this); + } + + /** + * Decorates this command with a set of commands to run after it in sequence. Often more + * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param next the commands to run next + * @return the decorated command + */ + default Command andThen(Command... next) { + SequentialCommandGroup group = new SequentialCommandGroup(this); + group.addCommands(next); + return group; + } + + /** + * Decorates this command with a set of commands to run parallel to it, ending when the calling + * command ends and interrupting all the others. Often more convenient/less-verbose than + * constructing a new {@link ParallelDeadlineGroup} explicitly. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param parallel the commands to run in parallel + * @return the decorated command + */ + default Command deadlineWith(Command... parallel) { + return new ParallelDeadlineGroup(this, parallel); + } + + /** + * Decorates this command with a set of commands to run parallel to it, ending when the last + * command ends. Often more convenient/less-verbose than constructing a new + * {@link ParallelCommandGroup} explicitly. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param parallel the commands to run in parallel + * @return the decorated command + */ + default Command alongWith(Command... parallel) { + ParallelCommandGroup group = new ParallelCommandGroup(this); + group.addCommands(parallel); + return group; + } + + /** + * Decorates this command with a set of commands to run parallel to it, ending when the first + * command ends. Often more convenient/less-verbose than constructing a new + * {@link ParallelRaceGroup} explicitly. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @param parallel the commands to run in parallel + * @return the decorated command + */ + default Command raceWith(Command... parallel) { + ParallelRaceGroup group = new ParallelRaceGroup(this); + group.addCommands(parallel); + return group; + } + + /** + * Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated + * command can still be interrupted or canceled. + * + *

Note: This decorator works by composing this command within a CommandGroup. The command + * cannot be used independently after being decorated, or be re-decorated with a different + * decorator, unless it is manually cleared from the list of grouped commands with + * {@link CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be + * further decorated without issue. + * + * @return the decorated command + */ + default Command perpetually() { + return new PerpetualCommand(this); + } + + /** + * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}. + * This is useful for "forking off" from command groups when the user does not wish to extend + * the command's requirements to the entire command group. + * + * @return the decorated command + */ + default Command asProxy() { + return new ProxyScheduleCommand(this); + } + + /** + * Schedules this command. + * + * @param interruptible whether this command can be interrupted by another command that + * shares one of its requirements + */ + default void schedule(boolean interruptible) { + CommandScheduler.getInstance().schedule(interruptible, this); + } + + /** + * Schedules this command, defaulting to interruptible. + */ + default void schedule() { + schedule(true); + } + + /** + * Cancels this command. Will call the command's interrupted() method. + * Commands will be canceled even if they are not marked as interruptible. + */ + default void cancel() { + CommandScheduler.getInstance().cancel(this); + } + + /** + * Whether or not the command is currently scheduled. Note that this does not detect whether + * the command is being run by a CommandGroup, only whether it is directly being run by + * the scheduler. + * + * @return Whether the command is scheduled. + */ + default boolean isScheduled() { + return CommandScheduler.getInstance().isScheduled(this); + } + + /** + * Whether the command requires a given subsystem. Named "hasRequirement" rather than "requires" + * to avoid confusion with + * {@link edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)} + * - this may be able to be changed in a few years. + * + * @param requirement the subsystem to inquire about + * @return whether the subsystem is required + */ + default boolean hasRequirement(Subsystem requirement) { + return getRequirements().contains(requirement); + } + + /** + * Whether the given command should run when the robot is disabled. Override to return true + * if the command should run when disabled. + * + * @return whether the command should run when the robot is disabled + */ + default boolean runsWhenDisabled() { + return false; + } + + default String getName() { + return this.getClass().getSimpleName(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java new file mode 100644 index 0000000000..0f706173c3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.HashSet; +import java.util.Set; + +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + +/** + * A {@link Sendable} base class for {@link Command}s. + */ +@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod") +public abstract class CommandBase implements Sendable, Command { + + protected String m_name = this.getClass().getSimpleName(); + protected String m_subsystem = "Ungrouped"; + protected Set m_requirements = new HashSet<>(); + + /** + * Adds the specified requirements to the command. + * + * @param requirements the requirements to add + */ + public final void addRequirements(Subsystem... requirements) { + m_requirements.addAll(Set.of(requirements)); + } + + @Override + public Set getRequirements() { + return m_requirements; + } + + @Override + public String getName() { + return this.getClass().getSimpleName(); + } + + @Override + public void setName(String name) { + m_name = name; + } + + @Override + public String getSubsystem() { + return m_subsystem; + } + + @Override + public void setSubsystem(String subsystem) { + m_subsystem = subsystem; + } + + /** + * Initializes this sendable. Useful for allowing implementations to easily extend SendableBase. + * + * @param builder the builder used to construct this sendable + */ + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Command"); + builder.addStringProperty(".name", this::getName, null); + builder.addBooleanProperty("running", this::isScheduled, value -> { + if (value) { + if (!isScheduled()) { + schedule(); + } + } else { + if (isScheduled()) { + cancel(); + } + } + }); + builder.addBooleanProperty(".isParented", + () -> CommandGroupBase.getGroupedCommands().contains(this), null); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java new file mode 100644 index 0000000000..6ac066742a --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java @@ -0,0 +1,124 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Collection; +import java.util.Collections; +import java.util.Set; +import java.util.WeakHashMap; + +/** + * A base for CommandGroups. Statically tracks commands that have been allocated to groups to + * ensure those commands are not also used independently, which can result in inconsistent command + * state and unpredictable execution. + */ +public abstract class CommandGroupBase extends CommandBase implements Command { + private static final Set m_groupedCommands = + Collections.newSetFromMap(new WeakHashMap<>()); + + static void registerGroupedCommands(Command... commands) { + m_groupedCommands.addAll(Set.of(commands)); + } + + /** + * Clears the list of grouped commands, allowing all commands to be freely used again. + * + *

WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not + * use this unless you fully understand what you are doing. + */ + public static void clearGroupedCommands() { + m_groupedCommands.clear(); + } + + /** + * Removes a single command from the list of grouped commands, allowing it to be freely used + * again. + * + *

WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not + * use this unless you fully understand what you are doing. + * + * @param command the command to remove from the list of grouped commands + */ + public static void clearGroupedCommand(Command command) { + m_groupedCommands.remove(command); + } + + /** + * Requires that the specified commands not have been already allocated to a CommandGroup. Throws + * an {@link IllegalArgumentException} if commands have been allocated. + * + * @param commands The commands to check + */ + public static void requireUngrouped(Command... commands) { + requireUngrouped(Set.of(commands)); + } + + /** + * Requires that the specified commands not have been already allocated to a CommandGroup. Throws + * an {@link IllegalArgumentException} if commands have been allocated. + * + * @param commands The commands to check + */ + public static void requireUngrouped(Collection commands) { + if (!Collections.disjoint(commands, getGroupedCommands())) { + throw new IllegalArgumentException("Commands cannot be added to more than one CommandGroup"); + } + } + + static Set getGroupedCommands() { + return m_groupedCommands; + } + + /** + * Adds the given commands to the command group. + * + * @param commands The commands to add. + */ + public abstract void addCommands(Command... commands); + + /** + * Factory method for {@link SequentialCommandGroup}, included for brevity/convenience. + * + * @param commands the commands to include + * @return the command group + */ + public static CommandGroupBase sequence(Command... commands) { + return new SequentialCommandGroup(commands); + } + + /** + * Factory method for {@link ParallelCommandGroup}, included for brevity/convenience. + * + * @param commands the commands to include + * @return the command group + */ + public static CommandGroupBase parallel(Command... commands) { + return new ParallelCommandGroup(commands); + } + + /** + * Factory method for {@link ParallelRaceGroup}, included for brevity/convenience. + * + * @param commands the commands to include + * @return the command group + */ + public static CommandGroupBase race(Command... commands) { + return new ParallelRaceGroup(commands); + } + + /** + * Factory method for {@link ParallelDeadlineGroup}, included for brevity/convenience. + * + * @param deadline the deadline command + * @param commands the commands to include + * @return the command group + */ + public static CommandGroupBase deadline(Command deadline, Command... commands) { + return new ParallelDeadlineGroup(deadline, commands); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java new file mode 100644 index 0000000000..1097811efa --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java @@ -0,0 +1,477 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.ArrayList; +import java.util.Collection; +import java.util.Collections; +import java.util.Iterator; +import java.util.LinkedHashMap; +import java.util.LinkedHashSet; +import java.util.List; +import java.util.Map; +import java.util.Set; +import java.util.function.Consumer; + +import edu.wpi.first.hal.FRCNetComm.tInstances; +import edu.wpi.first.hal.FRCNetComm.tResourceType; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.SendableBase; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + +/** + * The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link + * CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands + * synchronously from the main loop. Subsystems should be registered with the scheduler using + * {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link + * Subsystem#periodic()} methods to be called and for their default commands to be scheduled. + */ +@SuppressWarnings({"PMD.GodClass", "PMD.TooManyMethods", "PMD.TooManyFields"}) +public final class CommandScheduler extends SendableBase { + /** + * The Singleton Instance. + */ + private static CommandScheduler instance; + + /** + * Returns the Scheduler instance. + * + * @return the instance + */ + public static synchronized CommandScheduler getInstance() { + if (instance == null) { + instance = new CommandScheduler(); + } + return instance; + } + + //A map from commands to their scheduling state. Also used as a set of the currently-running + //commands. + private final Map m_scheduledCommands = new LinkedHashMap<>(); + + //A map from required subsystems to their requiring commands. Also used as a set of the + //currently-required subsystems. + private final Map m_requirements = new LinkedHashMap<>(); + + //A map from subsystems registered with the scheduler to their default commands. Also used + //as a list of currently-registered subsystems. + private final Map m_subsystems = new LinkedHashMap<>(); + + //The set of currently-registered buttons that will be polled every iteration. + private final Collection m_buttons = new LinkedHashSet<>(); + + private boolean m_disabled; + + //NetworkTable entries for use in Sendable impl + private NetworkTableEntry m_namesEntry; + private NetworkTableEntry m_idsEntry; + private NetworkTableEntry m_cancelEntry; + + //Lists of user-supplied actions to be executed on scheduling events for every command. + private final List> m_initActions = new ArrayList<>(); + private final List> m_executeActions = new ArrayList<>(); + private final List> m_interruptActions = new ArrayList<>(); + private final List> m_finishActions = new ArrayList<>(); + + CommandScheduler() { + HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler); + setName("Scheduler"); + } + + /** + * Adds a button binding to the scheduler, which will be polled to schedule commands. + * + * @param button The button to add + */ + public void addButton(Runnable button) { + m_buttons.add(button); + } + + /** + * Removes all button bindings from the scheduler. + */ + public void clearButtons() { + m_buttons.clear(); + } + + /** + * Initializes a given command, adds its requirements to the list, and performs the init actions. + * + * @param command The command to initialize + * @param interruptible Whether the command is interruptible + * @param requirements The command requirements + */ + private void initCommand(Command command, boolean interruptible, Set requirements) { + command.initialize(); + CommandState scheduledCommand = new CommandState(interruptible); + m_scheduledCommands.put(command, scheduledCommand); + for (Consumer action : m_initActions) { + action.accept(command); + } + for (Subsystem requirement : requirements) { + m_requirements.put(requirement, command); + } + } + + /** + * Schedules a command for execution. Does nothing if the command is already scheduled. If a + * command's requirements are not available, it will only be started if all the commands currently + * using those requirements have been scheduled as interruptible. If this is the case, they will + * be interrupted and the command will be scheduled. + * + * @param interruptible whether this command can be interrupted + * @param command the command to schedule + */ + @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"}) + private void schedule(boolean interruptible, Command command) { + if (CommandGroupBase.getGroupedCommands().contains(command)) { + throw new IllegalArgumentException( + "A command that is part of a command group cannot be independently scheduled"); + } + + //Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't + //run when disabled, or the command is already scheduled. + if (m_disabled || (RobotState.isDisabled() && !command.runsWhenDisabled()) + || m_scheduledCommands.containsKey(command)) { + return; + } + + Set requirements = command.getRequirements(); + + //Schedule the command if the requirements are not currently in-use. + if (Collections.disjoint(m_requirements.keySet(), requirements)) { + initCommand(command, interruptible, requirements); + } else { + //Else check if the requirements that are in use have all have interruptible commands, + //and if so, interrupt those commands and schedule the new command. + for (Subsystem requirement : requirements) { + if (m_requirements.containsKey(requirement) + && !m_scheduledCommands.get(m_requirements.get(requirement)).isInterruptible()) { + return; + } + } + for (Subsystem requirement : requirements) { + if (m_requirements.containsKey(requirement)) { + cancel(m_requirements.get(requirement)); + } + } + initCommand(command, interruptible, requirements); + } + } + + /** + * Schedules multiple commands for execution. Does nothing if the command is already scheduled. + * If a command's requirements are not available, it will only be started if all the commands + * currently using those requirements have been scheduled as interruptible. If this is the case, + * they will be interrupted and the command will be scheduled. + * + * @param interruptible whether the commands should be interruptible + * @param commands the commands to schedule + */ + public void schedule(boolean interruptible, Command... commands) { + for (Command command : commands) { + schedule(interruptible, command); + } + } + + /** + * Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing + * if the command is already scheduled. + * + * @param commands the commands to schedule + */ + public void schedule(Command... commands) { + schedule(true, commands); + } + + /** + * Runs a single iteration of the scheduler. The execution occurs in the following order: + * + *

Subsystem periodic methods are called. + * + *

Button bindings are polled, and new commands are scheduled from them. + * + *

Currently-scheduled commands are executed. + * + *

End conditions are checked on currently-scheduled commands, and commands that are finished + * have their end methods called and are removed. + * + *

Any subsystems not being used as requirements have their default methods started. + */ + @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"}) + public void run() { + if (m_disabled) { + return; + } + + //Run the periodic method of all registered subsystems. + for (Subsystem subsystem : m_subsystems.keySet()) { + subsystem.periodic(); + } + + //Poll buttons for new commands to add. + for (Runnable button : m_buttons) { + button.run(); + } + + //Run scheduled commands, remove finished commands. + for (Iterator iterator = m_scheduledCommands.keySet().iterator(); + iterator.hasNext(); ) { + Command command = iterator.next(); + + if (!command.runsWhenDisabled() && RobotState.isDisabled()) { + command.end(true); + for (Consumer action : m_interruptActions) { + action.accept(command); + } + m_requirements.keySet().removeAll(command.getRequirements()); + iterator.remove(); + continue; + } + + command.execute(); + for (Consumer action : m_executeActions) { + action.accept(command); + } + if (command.isFinished()) { + command.end(false); + for (Consumer action : m_finishActions) { + action.accept(command); + } + iterator.remove(); + + m_requirements.keySet().removeAll(command.getRequirements()); + } + } + + //Add default commands for un-required registered subsystems. + for (Map.Entry subsystemCommand : m_subsystems.entrySet()) { + if (!m_requirements.containsKey(subsystemCommand.getKey()) + && subsystemCommand.getValue() != null) { + schedule(subsystemCommand.getValue()); + } + } + } + + /** + * Registers subsystems with the scheduler. This must be called for the subsystem's periodic + * block to run when the scheduler is run, and for the subsystem's default command to be + * scheduled. It is recommended to call this from the constructor of your subsystem + * implementations. + * + * @param subsystems the subsystem to register + */ + public void registerSubsystem(Subsystem... subsystems) { + for (Subsystem subsystem : subsystems) { + m_subsystems.put(subsystem, null); + } + } + + /** + * Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic + * block called, and will not have its default command scheduled. + * + * @param subsystems the subsystem to un-register + */ + public void unregisterSubsystem(Subsystem... subsystems) { + m_subsystems.keySet().removeAll(Set.of(subsystems)); + } + + /** + * Sets the default command for a subsystem. Registers that subsystem if it is not already + * registered. Default commands will run whenever there is no other command currently scheduled + * that requires the subsystem. Default commands should be written to never end (i.e. their + * {@link Command#isFinished()} method should return false), as they would simply be re-scheduled + * if they do. Default commands must also require their subsystem. + * + * @param subsystem the subsystem whose default command will be set + * @param defaultCommand the default command to associate with the subsystem + */ + public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) { + if (!defaultCommand.getRequirements().contains(subsystem)) { + throw new IllegalArgumentException("Default commands must require their subsystem!"); + } + + if (defaultCommand.isFinished()) { + throw new IllegalArgumentException("Default commands should not end!"); + } + + m_subsystems.put(subsystem, defaultCommand); + } + + /** + * Gets the default command associated with this subsystem. Null if this subsystem has no default + * command associated with it. + * + * @param subsystem the subsystem to inquire about + * @return the default command associated with the subsystem + */ + public Command getDefaultCommand(Subsystem subsystem) { + return m_subsystems.get(subsystem); + } + + /** + * Cancels commands. The scheduler will only call the interrupted method of a canceled command, + * not the end method (though the interrupted method may itself call the end method). Commands + * will be canceled even if they are not scheduled as interruptible. + * + * @param commands the commands to cancel + */ + public void cancel(Command... commands) { + for (Command command : commands) { + if (!m_scheduledCommands.containsKey(command)) { + continue; + } + + command.end(true); + for (Consumer action : m_interruptActions) { + action.accept(command); + } + m_scheduledCommands.remove(command); + m_requirements.keySet().removeAll(command.getRequirements()); + } + } + + /** + * Cancels all commands that are currently scheduled. + */ + public void cancelAll() { + for (Command command : m_scheduledCommands.keySet()) { + cancel(command); + } + } + + /** + * Returns the time since a given command was scheduled. Note that this only works on commands + * that are directly scheduled by the scheduler; it will not work on commands inside of + * commandgroups, as the scheduler does not see them. + * + * @param command the command to query + * @return the time since the command was scheduled, in seconds + */ + public double timeSinceScheduled(Command command) { + CommandState commandState = m_scheduledCommands.get(command); + if (commandState != null) { + return commandState.timeSinceInitialized(); + } else { + return -1; + } + } + + /** + * Whether the given commands are running. Note that this only works on commands that are + * directly scheduled by the scheduler; it will not work on commands inside of CommandGroups, as + * the scheduler does not see them. + * + * @param commands the command to query + * @return whether the command is currently scheduled + */ + public boolean isScheduled(Command... commands) { + return m_scheduledCommands.keySet().containsAll(Set.of(commands)); + } + + /** + * Returns the command currently requiring a given subsystem. Null if no command is currently + * requiring the subsystem + * + * @param subsystem the subsystem to be inquired about + * @return the command currently requiring the subsystem + */ + public Command requiring(Subsystem subsystem) { + return m_requirements.get(subsystem); + } + + /** + * Disables the command scheduler. + */ + public void disable() { + m_disabled = true; + } + + /** + * Enables the command scheduler. + */ + public void enable() { + m_disabled = false; + } + + /** + * Adds an action to perform on the initialization of any command by the scheduler. + * + * @param action the action to perform + */ + public void onCommandInitialize(Consumer action) { + m_initActions.add(action); + } + + /** + * Adds an action to perform on the execution of any command by the scheduler. + * + * @param action the action to perform + */ + public void onCommandExecute(Consumer action) { + m_executeActions.add(action); + } + + /** + * Adds an action to perform on the interruption of any command by the scheduler. + * + * @param action the action to perform + */ + public void onCommandInterrupt(Consumer action) { + m_interruptActions.add(action); + } + + /** + * Adds an action to perform on the finishing of any command by the scheduler. + * + * @param action the action to perform + */ + public void onCommandFinish(Consumer action) { + m_finishActions.add(action); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Scheduler"); + m_namesEntry = builder.getEntry("Names"); + m_idsEntry = builder.getEntry("Ids"); + m_cancelEntry = builder.getEntry("Cancel"); + builder.setUpdateTable(() -> { + + if (m_namesEntry == null || m_idsEntry == null || m_cancelEntry == null) { + return; + } + + Map ids = new LinkedHashMap<>(); + + + for (Command command : m_scheduledCommands.keySet()) { + ids.put((double) command.hashCode(), command); + } + + double[] toCancel = m_cancelEntry.getDoubleArray(new double[0]); + if (toCancel.length > 0) { + for (double hash : toCancel) { + cancel(ids.get(hash)); + ids.remove(hash); + } + m_cancelEntry.setDoubleArray(new double[0]); + } + + List names = new ArrayList<>(); + + ids.values().forEach(command -> names.add(command.getName())); + + m_namesEntry.setStringArray(names.toArray(new String[0])); + m_idsEntry.setNumberArray(ids.keySet().toArray(new Double[0])); + }); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java new file mode 100644 index 0000000000..2e2dc7e367 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import edu.wpi.first.wpilibj.Timer; + +/** + * Class that holds scheduling state for a command. Used internally by the + * {@link CommandScheduler}. + */ +class CommandState { + //The time since this command was initialized. + private double m_startTime = -1; + + //Whether or not it is interruptible. + private final boolean m_interruptible; + + CommandState(boolean interruptible) { + m_interruptible = interruptible; + startTiming(); + startRunning(); + } + + private void startTiming() { + m_startTime = Timer.getFPGATimestamp(); + } + + synchronized void startRunning() { + m_startTime = -1; + } + + boolean isInterruptible() { + return m_interruptible; + } + + double timeSinceInitialized() { + return m_startTime != -1 ? Timer.getFPGATimestamp() - m_startTime : -1; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java new file mode 100644 index 0000000000..8e3239c261 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java @@ -0,0 +1,82 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.BooleanSupplier; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped; + +/** + * Runs one of two commands, depending on the value of the given condition when this command is + * initialized. Does not actually schedule the selected command - rather, the command is run + * through this command; this ensures that the command will behave as expected if used as part of a + * CommandGroup. Requires the requirements of both commands, again to ensure proper functioning + * when used in a CommandGroup. If this is undesired, consider using {@link ScheduleCommand}. + * + *

As this command contains multiple component commands within it, it is technically a command + * group; the command instances that are passed to it cannot be added to any other groups, or + * scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class ConditionalCommand extends CommandBase { + private final Command m_onTrue; + private final Command m_onFalse; + private final BooleanSupplier m_condition; + private Command m_selectedCommand; + + /** + * Creates a new ConditionalCommand. + * + * @param onTrue the command to run if the condition is true + * @param onFalse the command to run if the condition is false + * @param condition the condition to determine which command to run + */ + public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) { + requireUngrouped(onTrue, onFalse); + + CommandGroupBase.registerGroupedCommands(onTrue, onFalse); + + m_onTrue = onTrue; + m_onFalse = onFalse; + m_condition = requireNonNullParam(condition, "condition", "ConditionalCommand"); + m_requirements.addAll(m_onTrue.getRequirements()); + m_requirements.addAll(m_onFalse.getRequirements()); + } + + @Override + public void initialize() { + if (m_condition.getAsBoolean()) { + m_selectedCommand = m_onTrue; + } else { + m_selectedCommand = m_onFalse; + } + m_selectedCommand.initialize(); + } + + @Override + public void execute() { + m_selectedCommand.execute(); + } + + @Override + public void end(boolean interrupted) { + m_selectedCommand.end(interrupted); + } + + @Override + public boolean isFinished() { + return m_selectedCommand.isFinished(); + } + + @Override + public boolean runsWhenDisabled() { + return m_onTrue.runsWhenDisabled() && m_onFalse.runsWhenDisabled(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java new file mode 100644 index 0000000000..b9dd6db5dd --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.BooleanSupplier; +import java.util.function.Consumer; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that allows the user to pass in functions for each of the basic command methods through + * the constructor. Useful for inline definitions of complex commands - note, however, that if a + * command is beyond a certain complexity it is usually better practice to write a proper class for + * it than to inline it. + */ +public class FunctionalCommand extends CommandBase { + protected final Runnable m_onInit; + protected final Runnable m_onExecute; + protected final Consumer m_onEnd; + protected final BooleanSupplier m_isFinished; + + /** + * Creates a new FunctionalCommand. + * + * @param onInit the function to run on command initialization + * @param onExecute the function to run on command execution + * @param onEnd the function to run on command end + * @param isFinished the function that determines whether the command has finished + * @param requirements the subsystems required by this command + */ + public FunctionalCommand(Runnable onInit, Runnable onExecute, Consumer onEnd, + BooleanSupplier isFinished, Subsystem... requirements) { + m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand"); + m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand"); + m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand"); + m_isFinished = requireNonNullParam(isFinished, "isFinished", "FunctionalCommand"); + + addRequirements(requirements); + } + + @Override + public void initialize() { + m_onInit.run(); + } + + @Override + public void execute() { + m_onExecute.run(); + } + + @Override + public void end(boolean interrupted) { + m_onEnd.accept(interrupted); + } + + @Override + public boolean isFinished() { + return m_isFinished.getAsBoolean(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java new file mode 100644 index 0000000000..851889084e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A Command that runs instantly; it will initialize, execute once, and end on the same + * iteration of the scheduler. Users can either pass in a Runnable and a set of requirements, + * or else subclass this command if desired. + */ +public class InstantCommand extends CommandBase { + private final Runnable m_toRun; + + /** + * Creates a new InstantCommand that runs the given Runnable with the given requirements. + * + * @param toRun the Runnable to run + * @param requirements the subsystems required by this command + */ + public InstantCommand(Runnable toRun, Subsystem... requirements) { + m_toRun = requireNonNullParam(toRun, "toRun", "InstantCommand"); + + addRequirements(requirements); + } + + /** + * Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg + * constructor to call implicitly from subclass constructors. + */ + public InstantCommand() { + m_toRun = () -> { + }; + } + + @Override + public void initialize() { + m_toRun.run(); + } + + @Override + public final boolean isFinished() { + return true; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java new file mode 100644 index 0000000000..80e74346a5 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.Notifier; + +/** + * A command that starts a notifier to run the given runnable periodically in a separate thread. + * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or + * {@link Command#interruptOn(BooleanSupplier)} to give it one. + * + *

WARNING: Do not use this class unless you are confident in your ability to make the executed + * code thread-safe. If you do not know what "thread-safe" means, that is a good sign that + * you should not use this class. + */ +public class NotifierCommand extends CommandBase { + protected final Notifier m_notifier; + protected final double m_period; + + /** + * Creates a new NotifierCommand. + * + * @param toRun the runnable for the notifier to run + * @param period the period at which the notifier should run, in seconds + * @param requirements the subsystems required by this command + */ + public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) { + m_notifier = new Notifier(toRun); + m_period = period; + addRequirements(requirements); + } + + @Override + public void initialize() { + m_notifier.startPeriodic(m_period); + } + + @Override + public void end(boolean interrupted) { + m_notifier.stop(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java new file mode 100644 index 0000000000..4e71f1cc74 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java @@ -0,0 +1,155 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Set; +import java.util.function.DoubleConsumer; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj.controller.PIDController; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that controls an output with a {@link PIDController}. Runs forever by default - to add + * exit conditions and/or other behavior, subclass this class. The controller calculation and + * output are performed synchronously in the command's execute() method. + */ +public class PIDCommand extends CommandBase { + protected final PIDController m_controller; + protected DoubleSupplier m_measurement; + protected DoubleSupplier m_setpoint; + protected DoubleConsumer m_useOutput; + + /** + * Creates a new PIDCommand, which controls the given output with a PIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpointSource the controller's setpoint + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + public PIDCommand(PIDController controller, DoubleSupplier measurementSource, + DoubleSupplier setpointSource, DoubleConsumer useOutput, + Subsystem... requirements) { + requireNonNullParam(controller, "controller", "SynchronousPIDCommand"); + requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand"); + requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand"); + requireNonNullParam(useOutput, "useOutput", "SynchronousPIDCommand"); + + m_controller = controller; + m_useOutput = useOutput; + m_measurement = measurementSource; + m_setpoint = setpointSource; + m_requirements.addAll(Set.of(requirements)); + } + + /** + * Creates a new PIDCommand, which controls the given output with a PIDController. + * + * @param controller the controller that controls the output. + * @param measurementSource the measurement of the process variable + * @param setpoint the controller's setpoint + * @param useOutput the controller's output + * @param requirements the subsystems required by this command + */ + public PIDCommand(PIDController controller, DoubleSupplier measurementSource, + double setpoint, DoubleConsumer useOutput, + Subsystem... requirements) { + this(controller, measurementSource, () -> setpoint, useOutput, requirements); + } + + @Override + public void initialize() { + m_controller.reset(); + } + + @Override + public void execute() { + useOutput(m_controller.calculate(getMeasurement(), getSetpoint())); + } + + @Override + public void end(boolean interrupted) { + useOutput(0); + } + + /** + * Sets the function that uses the output of the PIDController. + * + * @param useOutput The function that uses the output. + */ + public final void setOutput(DoubleConsumer useOutput) { + m_useOutput = useOutput; + } + + /** + * Returns the PIDController used by the command. + * + * @return The PIDController + */ + public PIDController getController() { + return m_controller; + } + + /** + * Sets the setpoint for the controller to track the given source. + * + * @param setpointSource The setpoint source + */ + public void setSetpoint(DoubleSupplier setpointSource) { + m_setpoint = setpointSource; + } + + /** + * Sets the setpoint for the controller to a constant value. + * + * @param setpoint The setpoint + */ + public void setSetpoint(double setpoint) { + setSetpoint(() -> setpoint); + } + + /** + * Sets the setpoint for the controller to a constant value relative (i.e. added to) the current + * setpoint. + * + * @param relativeReference The change in setpoint + */ + public void setSetpointRelative(double relativeReference) { + setSetpoint(m_controller.getSetpoint() + relativeReference); + } + + /** + * Gets the setpoint for the controller. Wraps the passed-in function for readability. + * + * @return The setpoint for the controller + */ + private double getSetpoint() { + return m_setpoint.getAsDouble(); + } + + /** + * Gets the measurement of the process variable. Wraps the passed-in function for readability. + * + * @return The measurement of the process variable + */ + private double getMeasurement() { + return m_measurement.getAsDouble(); + } + + /** + * Uses the output of the controller. Wraps the passed-in function for readability. + * + * @param output The output value to use + */ + private void useOutput(double output) { + m_useOutput.accept(output); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java new file mode 100644 index 0000000000..f853f253bd --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import edu.wpi.first.wpilibj.controller.PIDController; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A subsystem that uses a {@link PIDController} to control an output. The controller is run + * synchronously from the subsystem's periodic() method. + */ +public abstract class PIDSubsystem extends SubsystemBase { + protected final PIDController m_controller; + protected boolean m_enabled; + + /** + * Creates a new PIDSubsystem. + * + * @param controller the PIDController to use + */ + public PIDSubsystem(PIDController controller) { + requireNonNullParam(controller, "controller", "PIDSubsystem"); + m_controller = controller; + } + + @Override + public void periodic() { + if (m_enabled) { + useOutput(m_controller.calculate(getMeasurement(), getSetpoint())); + } + } + + public PIDController getController() { + return m_controller; + } + + /** + * Uses the output from the PIDController. + * + * @param output the output of the PIDController + */ + public abstract void useOutput(double output); + + /** + * Returns the reference (setpoint) used by the PIDController. + * + * @return the reference (setpoint) to be used by the controller + */ + public abstract double getSetpoint(); + + /** + * Returns the measurement of the process variable used by the PIDController. + * + * @return the measurement of the process variable + */ + public abstract double getMeasurement(); + + /** + * Enables the PID control. Resets the controller. + */ + public void enable() { + m_enabled = true; + m_controller.reset(); + } + + /** + * Disables the PID control. Sets output to zero. + */ + public void disable() { + m_enabled = false; + useOutput(0); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java new file mode 100644 index 0000000000..38cc3c12dc --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java @@ -0,0 +1,99 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; + +/** + * A CommandGroup that runs a set of commands in parallel, ending when the last command ends. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class ParallelCommandGroup extends CommandGroupBase { + //maps commands in this group to whether they are still running + private final Map m_commands = new HashMap<>(); + private boolean m_runWhenDisabled = true; + + /** + * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. + * The command group will finish when the last command finishes. If the CommandGroup is + * interrupted, only the commands that are still running will be interrupted. + * + * @param commands the commands to include in this group. + */ + public ParallelCommandGroup(Command... commands) { + addCommands(commands); + } + + @Override + public final void addCommands(Command... commands) { + requireUngrouped(commands); + + if (m_commands.containsValue(true)) { + throw new IllegalStateException( + "Commands cannot be added to a CommandGroup while the group is running"); + } + + registerGroupedCommands(commands); + + for (Command command : commands) { + if (!Collections.disjoint(command.getRequirements(), m_requirements)) { + throw new IllegalArgumentException("Multiple commands in a parallel group cannot" + + "require the same subsystems"); + } + m_commands.put(command, false); + m_requirements.addAll(command.getRequirements()); + m_runWhenDisabled &= command.runsWhenDisabled(); + } + } + + @Override + public void initialize() { + for (Map.Entry commandRunning : m_commands.entrySet()) { + commandRunning.getKey().initialize(); + commandRunning.setValue(true); + } + } + + @Override + public void execute() { + for (Map.Entry commandRunning : m_commands.entrySet()) { + if (!commandRunning.getValue()) { + continue; + } + commandRunning.getKey().execute(); + if (commandRunning.getKey().isFinished()) { + commandRunning.getKey().end(false); + commandRunning.setValue(false); + } + } + } + + @Override + public void end(boolean interrupted) { + if (interrupted) { + for (Map.Entry commandRunning : m_commands.entrySet()) { + if (commandRunning.getValue()) { + commandRunning.getKey().end(true); + } + } + } + } + + @Override + public boolean isFinished() { + return !m_commands.values().contains(true); + } + + @Override + public boolean runsWhenDisabled() { + return m_runWhenDisabled; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java new file mode 100644 index 0000000000..61517ef140 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java @@ -0,0 +1,118 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Collections; +import java.util.HashMap; +import java.util.Map; + +/** + * A CommandGroup that runs a set of commands in parallel, ending only when a specific command + * (the "deadline") ends, interrupting all other commands that are still running at that point. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class ParallelDeadlineGroup extends CommandGroupBase { + //maps commands in this group to whether they are still running + private final Map m_commands = new HashMap<>(); + private boolean m_runWhenDisabled = true; + private Command m_deadline; + + /** + * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be + * executed simultaneously. The CommandGroup will finish when the deadline finishes, + * interrupting all other still-running commands. If the CommandGroup is interrupted, only + * the commands still running will be interrupted. + * + * @param deadline the command that determines when the group ends + * @param commands the commands to be executed + */ + public ParallelDeadlineGroup(Command deadline, Command... commands) { + m_deadline = deadline; + addCommands(commands); + if (!m_commands.containsKey(deadline)) { + addCommands(deadline); + } + } + + /** + * Sets the deadline to the given command. The deadline is added to the group if it is not + * already contained. + * + * @param deadline the command that determines when the group ends + */ + public void setDeadline(Command deadline) { + if (!m_commands.containsKey(deadline)) { + addCommands(deadline); + } + m_deadline = deadline; + } + + @Override + public final void addCommands(Command... commands) { + requireUngrouped(commands); + + if (m_commands.containsValue(true)) { + throw new IllegalStateException( + "Commands cannot be added to a CommandGroup while the group is running"); + } + + registerGroupedCommands(commands); + + for (Command command : commands) { + if (!Collections.disjoint(command.getRequirements(), m_requirements)) { + throw new IllegalArgumentException("Multiple commands in a parallel group cannot" + + "require the same subsystems"); + } + m_commands.put(command, false); + m_requirements.addAll(command.getRequirements()); + m_runWhenDisabled &= command.runsWhenDisabled(); + } + } + + @Override + public void initialize() { + for (Map.Entry commandRunning : m_commands.entrySet()) { + commandRunning.getKey().initialize(); + commandRunning.setValue(true); + } + } + + @Override + public void execute() { + for (Map.Entry commandRunning : m_commands.entrySet()) { + if (!commandRunning.getValue()) { + continue; + } + commandRunning.getKey().execute(); + if (commandRunning.getKey().isFinished()) { + commandRunning.getKey().end(false); + commandRunning.setValue(false); + } + } + } + + @Override + public void end(boolean interrupted) { + for (Map.Entry commandRunning : m_commands.entrySet()) { + if (commandRunning.getValue()) { + commandRunning.getKey().end(true); + } + } + } + + @Override + public boolean isFinished() { + return m_deadline.isFinished(); + } + + @Override + public boolean runsWhenDisabled() { + return m_runWhenDisabled; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java new file mode 100644 index 0000000000..a84d8aaa2f --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Collections; +import java.util.HashSet; +import java.util.Set; + +/** + * A CommandGroup that runs a set of commands in parallel, ending when any one of the commands ends + * and interrupting all the others. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class ParallelRaceGroup extends CommandGroupBase { + private final Set m_commands = new HashSet<>(); + private boolean m_runWhenDisabled = true; + private boolean m_finished = true; + + /** + * Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and + * will "race to the finish" - the first command to finish ends the entire command, with all other + * commands being interrupted. + * + * @param commands the commands to include in this group. + */ + public ParallelRaceGroup(Command... commands) { + addCommands(commands); + } + + @Override + public final void addCommands(Command... commands) { + requireUngrouped(commands); + + if (!m_finished) { + throw new IllegalStateException( + "Commands cannot be added to a CommandGroup while the group is running"); + } + + registerGroupedCommands(commands); + + for (Command command : commands) { + if (!Collections.disjoint(command.getRequirements(), m_requirements)) { + throw new IllegalArgumentException("Multiple commands in a parallel group cannot" + + " require the same subsystems"); + } + m_commands.add(command); + m_requirements.addAll(command.getRequirements()); + m_runWhenDisabled &= command.runsWhenDisabled(); + } + } + + @Override + public void initialize() { + m_finished = false; + for (Command command : m_commands) { + command.initialize(); + } + } + + @Override + public void execute() { + for (Command command : m_commands) { + command.execute(); + if (command.isFinished()) { + m_finished = true; + command.end(false); + } + } + } + + @Override + public void end(boolean interrupted) { + for (Command command : m_commands) { + if (!command.isFinished()) { + command.end(true); + } + } + } + + @Override + public boolean isFinished() { + return m_finished; + } + + @Override + public boolean runsWhenDisabled() { + return m_runWhenDisabled; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java new file mode 100644 index 0000000000..6ebb3767a9 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.registerGroupedCommands; +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped; + +/** + * A command that runs another command in perpetuity, ignoring that command's end conditions. While + * this class does not extend {@link CommandGroupBase}, it is still considered a CommandGroup, as it + * allows one to compose another command within it; the command instances that are passed to it + * cannot be added to any other groups, or scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class PerpetualCommand extends CommandBase { + protected final Command m_command; + + /** + * Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that + * command's end conditions, unless this command itself is interrupted. + * + * @param command the command to run perpetually + */ + public PerpetualCommand(Command command) { + requireUngrouped(command); + registerGroupedCommands(command); + m_command = command; + m_requirements.addAll(command.getRequirements()); + } + + @Override + public void initialize() { + m_command.initialize(); + } + + @Override + public void execute() { + m_command.execute(); + } + + @Override + public void end(boolean interrupted) { + m_command.end(interrupted); + } + + @Override + public boolean runsWhenDisabled() { + return m_command.runsWhenDisabled(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java new file mode 100644 index 0000000000..4fb4126499 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +/** + * A command that prints a string when initialized. + */ +public class PrintCommand extends InstantCommand { + /** + * Creates a new a PrintCommand. + * + * @param message the message to print + */ + public PrintCommand(String message) { + super(() -> System.out.println(message)); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java new file mode 100644 index 0000000000..27d67bcee3 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Set; + +/** + * Schedules the given commands when this command is initialized, and ends when all the commands are + * no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted, + * it will cancel all of the commands. + */ +public class ProxyScheduleCommand extends CommandBase { + private final Set m_toSchedule; + private boolean m_finished; + + /** + * Creates a new ProxyScheduleCommand that schedules the given commands when initialized, + * and ends when they are all no longer scheduled. + * + * @param toSchedule the commands to schedule + */ + public ProxyScheduleCommand(Command... toSchedule) { + m_toSchedule = Set.of(toSchedule); + } + + @Override + public void initialize() { + for (Command command : m_toSchedule) { + command.schedule(); + } + } + + @Override + public void end(boolean interrupted) { + if (interrupted) { + for (Command command : m_toSchedule) { + command.cancel(); + } + } + } + + @Override + public void execute() { + m_finished = true; + for (Command command : m_toSchedule) { + m_finished &= !command.isScheduled(); + } + } + + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java new file mode 100644 index 0000000000..fe302813f2 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.BooleanSupplier; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that runs a Runnable continuously. Has no end condition as-is; + * either subclass it or use {@link Command#withTimeout(double)} or + * {@link Command#interruptOn(BooleanSupplier)} to give it one. If you only wish + * to execute a Runnable once, use {@link InstantCommand}. + */ +public class RunCommand extends CommandBase { + protected final Runnable m_toRun; + + /** + * Creates a new RunCommand. The Runnable will be run continuously until the command + * ends. Does not run when disabled. + * + * @param toRun the Runnable to run + * @param requirements the subsystems to require + */ + public RunCommand(Runnable toRun, Subsystem... requirements) { + m_toRun = requireNonNullParam(toRun, "toRun", "RunCommand"); + addRequirements(requirements); + } + + @Override + public void execute() { + m_toRun.run(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java new file mode 100644 index 0000000000..700925b5e2 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Set; + +/** + * Schedules the given commands when this command is initialized. Useful for forking off from + * CommandGroups. Note that if run from a CommandGroup, the group will not know about the status + * of the scheduled commands, and will treat this command as finishing instantly. + */ +public class ScheduleCommand extends CommandBase { + private final Set m_toSchedule; + + /** + * Creates a new ScheduleCommand that schedules the given commands when initialized. + * + * @param toSchedule the commands to schedule + */ + public ScheduleCommand(Command... toSchedule) { + m_toSchedule = Set.of(toSchedule); + } + + @Override + public void initialize() { + for (Command command : m_toSchedule) { + command.schedule(); + } + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java new file mode 100644 index 0000000000..92fe07f89b --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Map; +import java.util.function.Supplier; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped; + +/** + * Runs one of a selection of commands, either using a selector and a key to command mapping, or a + * supplier that returns the command directly at runtime. Does not actually schedule the selected + * command - rather, the command is run through this command; this ensures that the command will + * behave as expected if used as part of a CommandGroup. Requires the requirements of all included + * commands, again to ensure proper functioning when used in a CommandGroup. If this is undesired, + * consider using {@link ScheduleCommand}. + * + *

As this command contains multiple component commands within it, it is technically a command + * group; the command instances that are passed to it cannot be added to any other groups, or + * scheduled individually. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class SelectCommand extends CommandBase { + private final Map m_commands; + private final Supplier m_selector; + private final Supplier m_toRun; + private Command m_selectedCommand; + + /** + * Creates a new selectcommand. + * + * @param commands the map of commands to choose from + * @param selector the selector to determine which command to run + */ + public SelectCommand(Map commands, Supplier selector) { + requireUngrouped(commands.values()); + + CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{})); + + m_commands = requireNonNullParam(commands, "commands", "SelectCommand"); + m_selector = requireNonNullParam(selector, "selector", "SelectCommand"); + + m_toRun = null; + + for (Command command : m_commands.values()) { + m_requirements.addAll(command.getRequirements()); + } + } + + /** + * Creates a new selectcommand. + * + * @param toRun a supplier providing the command to run + */ + public SelectCommand(Supplier toRun) { + m_commands = null; + m_selector = null; + m_toRun = requireNonNullParam(toRun, "toRun", "SelectCommand"); + } + + @Override + public void initialize() { + if (m_selector != null) { + if (!m_commands.keySet().contains(m_selector.get())) { + m_selectedCommand = new PrintCommand( + "SelectCommand selector value does not correspond to" + " any command!"); + return; + } + m_selectedCommand = m_commands.get(m_selector.get()); + } else { + m_selectedCommand = m_toRun.get(); + } + m_selectedCommand.initialize(); + } + + @Override + public void execute() { + m_selectedCommand.execute(); + } + + @Override + public void end(boolean interrupted) { + m_selectedCommand.end(interrupted); + } + + @Override + public boolean isFinished() { + return m_selectedCommand.isFinished(); + } + + @Override + public boolean runsWhenDisabled() { + if (m_commands != null) { + boolean runsWhenDisabled = true; + for (Command command : m_commands.values()) { + runsWhenDisabled &= command.runsWhenDisabled(); + } + return runsWhenDisabled; + } else { + return m_toRun.get().runsWhenDisabled(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java new file mode 100644 index 0000000000..d834018796 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.ArrayList; +import java.util.List; + +/** + * A CommandGroups that runs a list of commands in sequence. + * + *

As a rule, CommandGroups require the union of the requirements of their component commands. + */ +public class SequentialCommandGroup extends CommandGroupBase { + private final List m_commands = new ArrayList<>(); + private int m_currentCommandIndex = -1; + private boolean m_runWhenDisabled = true; + + /** + * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with + * the CommandGroup finishing when the last command finishes. + * + * @param commands the commands to include in this group. + */ + public SequentialCommandGroup(Command... commands) { + addCommands(commands); + } + + @Override + public final void addCommands(Command... commands) { + requireUngrouped(commands); + + if (m_currentCommandIndex != -1) { + throw new IllegalStateException( + "Commands cannot be added to a CommandGroup while the group is running"); + } + + registerGroupedCommands(commands); + + for (Command command : commands) { + m_commands.add(command); + m_requirements.addAll(command.getRequirements()); + m_runWhenDisabled &= command.runsWhenDisabled(); + } + } + + @Override + public void initialize() { + m_currentCommandIndex = 0; + + if (!m_commands.isEmpty()) { + m_commands.get(0).initialize(); + } + } + + @Override + public void execute() { + if (m_commands.isEmpty()) { + return; + } + + Command currentCommand = m_commands.get(m_currentCommandIndex); + + currentCommand.execute(); + if (currentCommand.isFinished()) { + currentCommand.end(false); + m_currentCommandIndex++; + if (m_currentCommandIndex < m_commands.size()) { + m_commands.get(m_currentCommandIndex).initialize(); + } + } + } + + @Override + public void end(boolean interrupted) { + if (interrupted && !m_commands.isEmpty()) { + m_commands.get(m_currentCommandIndex).end(true); + } + m_currentCommandIndex = -1; + } + + @Override + public boolean isFinished() { + return m_currentCommandIndex == m_commands.size(); + } + + @Override + public boolean runsWhenDisabled() { + return m_runWhenDisabled; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java new file mode 100644 index 0000000000..8e4fa91522 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.function.BooleanSupplier; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that runs a given runnable when it is initalized, and another runnable when it ends. + * Useful for running and then stopping a motor, or extending and then retracting a solenoid. + * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or + * {@link Command#interruptOn(BooleanSupplier)} to give it one. + */ +public class StartEndCommand extends CommandBase { + protected final Runnable m_onInit; + protected final Runnable m_onEnd; + + /** + * Creates a new StartEndCommand. Will run the given runnables when the command starts and when + * it ends. + * + * @param onInit the Runnable to run on command init + * @param onEnd the Runnable to run on command end + * @param requirements the subsystems required by this command + */ + public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) { + m_onInit = requireNonNullParam(onInit, "onInit", "StartEndCommand"); + m_onEnd = requireNonNullParam(onEnd, "onEnd", "StartEndCommand"); + + addRequirements(requirements); + } + + @Override + public void initialize() { + m_onInit.run(); + } + + @Override + public void end(boolean interrupted) { + m_onEnd.run(); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java new file mode 100644 index 0000000000..7dc917b3a5 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java @@ -0,0 +1,76 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +/** + * A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based + * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc) and + * provide methods through which they can be used by {@link Command}s. Subsystems are used by the + * {@link CommandScheduler}'s resource management system to ensure multiple robot actions are not + * "fighting" over the same hardware; Commands that use a subsystem should include that subsystem + * in their {@link Command#getRequirements()} method, and resources used within a subsystem should + * generally remain encapsulated and not be shared by other parts of the robot. + * + *

Subsystems must be registered with the scheduler with the + * {@link CommandScheduler#registerSubsystem(Subsystem...)} method in order for the + * {@link Subsystem#periodic()} method to be called. It is recommended that this method be called + * from the constructor of users' Subsystem implementations. The {@link SubsystemBase} + * class offers a simple base for user implementations that handles this. + */ +public interface Subsystem { + + /** + * This method is called periodically by the {@link CommandScheduler}. Useful for updating + * subsystem-specific state that you don't want to offload to a {@link Command}. Teams should + * try to be consistent within their own codebases about which responsibilities will be handled + * by Commands, and which will be handled here. + */ + default void periodic() { + } + + /** + * Sets the default {@link Command} of the subsystem. The default command will be + * automatically scheduled when no other commands are scheduled that require the subsystem. + * Default commands should generally not end on their own, i.e. their {@link Command#isFinished()} + * method should always return false. Will automatically register this subsystem with the + * {@link CommandScheduler}. + * + * @param defaultCommand the default command to associate with this subsystem + */ + default void setDefaultCommand(Command defaultCommand) { + CommandScheduler.getInstance().setDefaultCommand(this, defaultCommand); + } + + /** + * Gets the default command for this subsystem. Returns null if no default command is + * currently associated with the subsystem. + * + * @return the default command associated with this subsystem + */ + default Command getDefaultCommand() { + return CommandScheduler.getInstance().getDefaultCommand(this); + } + + /** + * Returns the command currently running on this subsystem. Returns null if no command is + * currently scheduled that requires this subsystem. + * + * @return the scheduled command currently requiring this subsystem + */ + default Command getCurrentCommand() { + return CommandScheduler.getInstance().requiring(this); + } + + /** + * Registers this subsystem with the {@link CommandScheduler}, allowing its + * {@link Subsystem#periodic()} method to be called when the scheduler runs. + */ + default void register() { + CommandScheduler.getInstance().registerSubsystem(this); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java new file mode 100644 index 0000000000..55249cee6e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import edu.wpi.first.wpilibj.Sendable; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder; + +/** + * A base for subsystems that handles registration in the constructor, and provides a more intuitive + * method for setting the default command. + */ +public abstract class SubsystemBase implements Subsystem, Sendable { + + protected String m_name = this.getClass().getSimpleName(); + + public SubsystemBase() { + CommandScheduler.getInstance().registerSubsystem(this); + } + + @Override + public String getName() { + return m_name; + } + + @Override + public void setName(String name) { + m_name = name; + } + + @Override + public String getSubsystem() { + return getName(); + } + + @Override + public void setSubsystem(String subsystem) { + setName(subsystem); + } + + /** + * Associates a {@link Sendable} with this Subsystem. + * Also update the child's name. + * + * @param name name to give child + * @param child sendable + */ + public void addChild(String name, Sendable child) { + child.setName(getSubsystem(), name); + LiveWindow.add(child); + } + + @Override + public void initSendable(SendableBuilder builder) { + builder.setSmartDashboardType("Subsystem"); + + builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null); + builder.addStringProperty(".default", + () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", null); + builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null); + builder.addStringProperty(".command", + () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none", null); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java new file mode 100644 index 0000000000..09ec2f8cd0 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import edu.wpi.first.wpilibj.Timer; + +/** + * A command that does nothing but takes a specified amount of time to finish. Useful for + * CommandGroups. Can also be subclassed to make a command with an internal timer. + */ +public class WaitCommand extends CommandBase { + protected Timer m_timer = new Timer(); + private final double m_duration; + + /** + * Creates a new WaitCommand. This command will do nothing, and end after the specified duration. + * + * @param seconds the time to wait, in seconds + */ + public WaitCommand(double seconds) { + m_duration = seconds; + setName(m_name + ": " + seconds + " seconds"); + } + + @Override + public void initialize() { + m_timer.reset(); + m_timer.start(); + } + + @Override + public void end(boolean interrupted) { + m_timer.stop(); + } + + @Override + public boolean isFinished() { + return m_timer.hasPeriodPassed(m_duration); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java new file mode 100644 index 0000000000..5c55fff1b5 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj.Timer; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A command that does nothing but ends after a specified match time or condition. Useful for + * CommandGroups. + */ +public class WaitUntilCommand extends CommandBase { + private final BooleanSupplier m_condition; + + /** + * Creates a new WaitUntilCommand that ends after a given condition becomes true. + * + * @param condition the condition to determine when to end + */ + public WaitUntilCommand(BooleanSupplier condition) { + m_condition = requireNonNullParam(condition, "condition", "WaitUntilCommand"); + } + + /** + * Creates a new WaitUntilCommand that ends after a given match time. + * + *

NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT + * guarantee that the time at which the action is performed will be judged to be legal by the + * referees. When in doubt, add a safety factor or time the action manually. + * + * @param time the match time after which to end, in seconds + */ + public WaitUntilCommand(double time) { + this(() -> Timer.getMatchTime() - time > 0); + } + + @Override + public boolean isFinished() { + return m_condition.getAsBoolean(); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java new file mode 100644 index 0000000000..d4ff65787e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java @@ -0,0 +1,211 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command.button; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/** + * This class provides an easy way to link commands to OI inputs. + * + *

It is very easy to link a button to a command. For instance, you could link the trigger + * button of a joystick to a "score" command. + * + *

This class represents a subclass of Trigger that is specifically aimed at buttons on an + * operator interface as a common use case of the more generalized Trigger objects. This is a simple + * wrapper around Trigger with the method names renamed to fit the Button object use. + */ +@SuppressWarnings("PMD.TooManyMethods") +public abstract class Button extends Trigger { + /** + * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is + * overridden). + */ + public Button() { + } + + /** + * Creates a new button with the given condition determining whether it is pressed. + * + * @param isPressed returns whether or not the trigger should be active + */ + public Button(BooleanSupplier isPressed) { + super(isPressed); + } + + /** + * Starts the given command whenever the button is newly pressed. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this button, so calls can be chained + */ + public Button whenPressed(final Command command, boolean interruptible) { + whenActive(command, interruptible); + return this; + } + + /** + * Starts the given command whenever the button is newly pressed. The command is set to be + * interruptible. + * + * @param command the command to start + * @return this button, so calls can be chained + */ + public Button whenPressed(final Command command) { + whenActive(command); + return this; + } + + /** + * Runs the given runnable whenever the button is newly pressed. + * + * @param toRun the runnable to run + * @return this button, so calls can be chained + */ + public Button whenPressed(final Runnable toRun) { + whenActive(toRun); + return this; + } + + /** + * Constantly starts the given command while the button is held. + * + * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will + * be canceled when the button is released. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this button, so calls can be chained + */ + public Button whileHeld(final Command command, boolean interruptible) { + whileActiveContinuous(command, interruptible); + return this; + } + + /** + * Constantly starts the given command while the button is held. + * + * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will + * be canceled when the button is released. The command is set to be interruptible. + * + * @param command the command to start + * @return this button, so calls can be chained + */ + public Button whileHeld(final Command command) { + whileActiveContinuous(command); + return this; + } + + /** + * Constantly runs the given runnable while the button is held. + * + * @param toRun the runnable to run + * @return this button, so calls can be chained + */ + public Button whileHeld(final Runnable toRun) { + whileActiveContinuous(toRun); + return this; + } + + /** + * Starts the given command when the button is first pressed, and cancels it when it is released, + * but does not start it again if it ends or is otherwise interrupted. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this button, so calls can be chained + */ + public Button whenHeld(final Command command, boolean interruptible) { + whileActiveOnce(command, interruptible); + return this; + } + + /** + * Starts the given command when the button is first pressed, and cancels it when it is released, + * but does not start it again if it ends or is otherwise interrupted. The command is set to be + * interruptible. + * + * @param command the command to start + * @return this button, so calls can be chained + */ + public Button whenHeld(final Command command) { + whileActiveOnce(command, true); + return this; + } + + + /** + * Starts the command when the button is released. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this button, so calls can be chained + */ + public Button whenReleased(final Command command, boolean interruptible) { + whenInactive(command, interruptible); + return this; + } + + /** + * Starts the command when the button is released. The command is set to be interruptible. + * + * @param command the command to start + * @return this button, so calls can be chained + */ + public Button whenReleased(final Command command) { + whenInactive(command); + return this; + } + + /** + * Runs the given runnable when the button is released. + * + * @param toRun the runnable to run + * @return this button, so calls can be chained + */ + public Button whenReleased(final Runnable toRun) { + whenInactive(toRun); + return this; + } + + /** + * Toggles the command whenever the button is pressed (on then off then on). + * + * @param command the command to start + * @param interruptible whether the command is interruptible + */ + public Button toggleWhenPressed(final Command command, boolean interruptible) { + toggleWhenActive(command, interruptible); + return this; + } + + /** + * Toggles the command whenever the button is pressed (on then off then on). The command is set + * to be interruptible. + * + * @param command the command to start + * @return this button, so calls can be chained + */ + public Button toggleWhenPressed(final Command command) { + toggleWhenActive(command); + return this; + } + + /** + * Cancels the command when the button is pressed. + * + * @param command the command to start + * @return this button, so calls can be chained + */ + public Button cancelWhenPressed(final Command command) { + cancelWhenActive(command); + return this; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java new file mode 100644 index 0000000000..3f74f4833d --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command.button; + +/** + * This class is intended to be used within a program. The programmer can manually set its value. + * Also includes a setting for whether or not it should invert its value. + */ +public class InternalButton extends Button { + private boolean m_pressed; + private boolean m_inverted; + + /** + * Creates an InternalButton that is not inverted. + */ + public InternalButton() { + this(false); + } + + /** + * Creates an InternalButton which is inverted depending on the input. + * + * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed + * when set to false. + */ + public InternalButton(boolean inverted) { + m_pressed = m_inverted = inverted; + } + + public void setInverted(boolean inverted) { + m_inverted = inverted; + } + + public void setPressed(boolean pressed) { + m_pressed = pressed; + } + + @Override + public boolean get() { + return m_pressed ^ m_inverted; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java new file mode 100644 index 0000000000..918bb6a56e --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command.button; + +import edu.wpi.first.wpilibj.GenericHID; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A {@link Button} that gets its state from a {@link GenericHID}. + */ +public class JoystickButton extends Button { + private final GenericHID m_joystick; + private final int m_buttonNumber; + + /** + * Creates a joystick button for triggering commands. + * + * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, + * etc) + * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) } + */ + public JoystickButton(GenericHID joystick, int buttonNumber) { + requireNonNullParam(joystick, "joystick", "JoystickButton"); + + m_joystick = joystick; + m_buttonNumber = buttonNumber; + } + + /** + * Gets the value of the joystick button. + * + * @return The value of the joystick button + */ + @Override + public boolean get() { + return m_joystick.getRawButton(m_buttonNumber); + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java new file mode 100644 index 0000000000..823b75643a --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command.button; + +import edu.wpi.first.wpilibj.GenericHID; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * A {@link Button} that gets its state from a POV on a {@link GenericHID}. + */ +public class POVButton extends Button { + private final GenericHID m_joystick; + private final int m_angle; + private final int m_povNumber; + + /** + * Creates a POV button for triggering commands. + * + * @param joystick The GenericHID object that has the POV + * @param angle The desired angle in degrees (e.g. 90, 270) + * @param povNumber The POV number (see {@link GenericHID#getPOV(int)}) + */ + public POVButton(GenericHID joystick, int angle, int povNumber) { + requireNonNullParam(joystick, "joystick", "POVButton"); + + m_joystick = joystick; + m_angle = angle; + m_povNumber = povNumber; + } + + /** + * Creates a POV button for triggering commands. + * By default, acts on POV 0 + * + * @param joystick The GenericHID object that has the POV + * @param angle The desired angle (e.g. 90, 270) + */ + public POVButton(GenericHID joystick, int angle) { + this(joystick, angle, 0); + } + + /** + * Checks whether the current value of the POV is the target angle. + * + * @return Whether the value of the POV matches the target angle + */ + @Override + public boolean get() { + return m_joystick.getPOV(m_povNumber) == m_angle; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java new file mode 100644 index 0000000000..9a12deff8f --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java @@ -0,0 +1,351 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2008-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command.button; + +import java.util.function.BooleanSupplier; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam; + +/** + * This class provides an easy way to link commands to inputs. + * + *

It is very easy to link a button to a command. For instance, you could link the trigger + * button of a joystick to a "score" command. + * + *

It is encouraged that teams write a subclass of Trigger if they want to have something + * unusual (for instance, if they want to react to the user holding a button while the robot is + * reading a certain sensor input). For this, they only have to write the {@link Trigger#get()} + * method to get the full functionality of the Trigger class. + */ +@SuppressWarnings("PMD.TooManyMethods") +public class Trigger { + private final BooleanSupplier m_isActive; + + /** + * Creates a new trigger with the given condition determining whether it is active. + * + * @param isActive returns whether or not the trigger should be active + */ + public Trigger(BooleanSupplier isActive) { + m_isActive = isActive; + } + + /** + * Creates a new trigger that is always inactive. Useful only as a no-arg constructor for + * subclasses that will be overriding {@link Trigger#get()} anyway. + */ + public Trigger() { + m_isActive = () -> false; + } + + /** + * Returns whether or not the trigger is active. + * + *

This method will be called repeatedly a command is linked to the Trigger. + * + * @return whether or not the trigger condition is active. + */ + public boolean get() { + return m_isActive.getAsBoolean(); + } + + /** + * Starts the given command whenever the trigger just becomes active. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this trigger, so calls can be chained + */ + public Trigger whenActive(final Command command, boolean interruptible) { + requireNonNullParam(command, "command", "whenActive"); + + CommandScheduler.getInstance().addButton(new Runnable() { + private boolean m_pressedLast = get(); + + @Override + public void run() { + boolean pressed = get(); + + if (!m_pressedLast && pressed) { + command.schedule(interruptible); + } + + m_pressedLast = pressed; + } + }); + + return this; + } + + /** + * Starts the given command whenever the trigger just becomes active. The command is set to be + * interruptible. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger whenActive(final Command command) { + return whenActive(command, true); + } + + /** + * Runs the given runnable whenever the trigger just becomes active. + * + * @param toRun the runnable to run + * @return this trigger, so calls can be chained + */ + public Trigger whenActive(final Runnable toRun) { + return whenActive(new InstantCommand(toRun)); + } + + /** + * Constantly starts the given command while the button is held. + * + * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and + * will be canceled when the trigger becomes inactive. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this trigger, so calls can be chained + */ + public Trigger whileActiveContinuous(final Command command, boolean interruptible) { + requireNonNullParam(command, "command", "whileActiveContinuous"); + + CommandScheduler.getInstance().addButton(new Runnable() { + private boolean m_pressedLast = get(); + + @Override + public void run() { + boolean pressed = get(); + + if (pressed) { + command.schedule(interruptible); + } else if (m_pressedLast) { + command.cancel(); + } + + m_pressedLast = pressed; + } + }); + return this; + } + + /** + * Constantly starts the given command while the button is held. + * + * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and + * will be canceled when the trigger becomes inactive. The command is set to be interruptible. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger whileActiveContinuous(final Command command) { + return whileActiveContinuous(command, true); + } + + /** + * Constantly runs the given runnable while the button is held. + * + * @param toRun the runnable to run + * @return this trigger, so calls can be chained + */ + public Trigger whileActiveContinuous(final Runnable toRun) { + return whileActiveContinuous(new InstantCommand(toRun)); + } + + /** + * Starts the given command when the trigger initially becomes active, and ends it when it becomes + * inactive, but does not re-start it in-between. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this trigger, so calls can be chained + */ + public Trigger whileActiveOnce(final Command command, boolean interruptible) { + requireNonNullParam(command, "command", "whileActiveOnce"); + + CommandScheduler.getInstance().addButton(new Runnable() { + private boolean m_pressedLast = get(); + + @Override + public void run() { + boolean pressed = get(); + + if (!m_pressedLast && pressed) { + command.schedule(interruptible); + } else if (m_pressedLast && !pressed) { + command.cancel(); + } + + m_pressedLast = pressed; + } + }); + return this; + } + + /** + * Starts the given command when the trigger initially becomes active, and ends it when it becomes + * inactive, but does not re-start it in-between. The command is set to be interruptible. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger whileActiveOnce(final Command command) { + return whileActiveOnce(command, true); + } + + /** + * Starts the command when the trigger becomes inactive. + * + * @param command the command to start + * @param interruptible whether the command is interruptible + * @return this trigger, so calls can be chained + */ + public Trigger whenInactive(final Command command, boolean interruptible) { + requireNonNullParam(command, "command", "whenInactive"); + + CommandScheduler.getInstance().addButton(new Runnable() { + private boolean m_pressedLast = get(); + + @Override + public void run() { + boolean pressed = get(); + + if (m_pressedLast && !pressed) { + command.schedule(interruptible); + } + + m_pressedLast = pressed; + } + }); + return this; + } + + /** + * Starts the command when the trigger becomes inactive. The command is set to be interruptible. + * + * @param command the command to start + * @return this trigger, so calls can be chained + */ + public Trigger whenInactive(final Command command) { + return whenInactive(command, true); + } + + /** + * Runs the given runnable when the trigger becomes inactive. + * + * @param toRun the runnable to run + * @return this trigger, so calls can be chained + */ + public Trigger whenInactive(final Runnable toRun) { + return whenInactive(new InstantCommand(toRun)); + } + + /** + * Toggles a command when the trigger becomes active. + * + * @param command the command to toggle + * @param interruptible whether the command is interruptible + * @return this trigger, so calls can be chained + */ + public Trigger toggleWhenActive(final Command command, boolean interruptible) { + requireNonNullParam(command, "command", "toggleWhenActive"); + + CommandScheduler.getInstance().addButton(new Runnable() { + private boolean m_pressedLast = get(); + + @Override + public void run() { + boolean pressed = get(); + + if (!m_pressedLast && pressed) { + if (command.isScheduled()) { + command.cancel(); + } else { + command.schedule(interruptible); + } + } + + m_pressedLast = pressed; + } + }); + return this; + } + + /** + * Toggles a command when the trigger becomes active. The command is set to be interruptible. + * + * @param command the command to toggle + * @return this trigger, so calls can be chained + */ + public Trigger toggleWhenActive(final Command command) { + return toggleWhenActive(command, true); + } + + /** + * Cancels a command when the trigger becomes active. + * + * @param command the command to cancel + * @return this trigger, so calls can be chained + */ + public Trigger cancelWhenActive(final Command command) { + requireNonNullParam(command, "command", "cancelWhenActive"); + + CommandScheduler.getInstance().addButton(new Runnable() { + private boolean m_pressedLast = get(); + + @Override + public void run() { + boolean pressed = get(); + + if (!m_pressedLast && pressed) { + command.cancel(); + } + + m_pressedLast = pressed; + } + }); + return this; + } + + /** + * Composes this trigger with another trigger, returning a new trigger that is active when both + * triggers are active. + * + * @param trigger the trigger to compose with + * @return the trigger that is active when both triggers are active + */ + public Trigger and(Trigger trigger) { + return new Trigger(() -> get() && trigger.get()); + } + + /** + * Composes this trigger with another trigger, returning a new trigger that is active when either + * trigger is active. + * + * @param trigger the trigger to compose with + * @return the trigger that is active when either trigger is active + */ + public Trigger or(Trigger trigger) { + return new Trigger(() -> get() || trigger.get()); + } + + /** + * Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the + * negation of this trigger. + * + * @return the negated trigger + */ + public Trigger negate() { + return new Trigger(() -> !get()); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java new file mode 100644 index 0000000000..64354c38f2 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ButtonTest.java @@ -0,0 +1,179 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj2.command.button.InternalButton; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.times; +import static org.mockito.Mockito.verify; +import static org.mockito.Mockito.when; + + +class ButtonTest extends CommandTestBase { + @Test + void whenPressedTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + InternalButton button = new InternalButton(); + button.setPressed(false); + button.whenPressed(command1); + scheduler.run(); + verify(command1, never()).schedule(true); + button.setPressed(true); + scheduler.run(); + scheduler.run(); + verify(command1).schedule(true); + } + + @Test + void whenReleasedTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + InternalButton button = new InternalButton(); + button.setPressed(true); + button.whenReleased(command1); + scheduler.run(); + verify(command1, never()).schedule(true); + button.setPressed(false); + scheduler.run(); + scheduler.run(); + verify(command1).schedule(true); + } + + @Test + void whileHeldTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + InternalButton button = new InternalButton(); + button.setPressed(false); + button.whileHeld(command1); + scheduler.run(); + verify(command1, never()).schedule(true); + button.setPressed(true); + scheduler.run(); + scheduler.run(); + verify(command1, times(2)).schedule(true); + button.setPressed(false); + scheduler.run(); + verify(command1).cancel(); + } + + @Test + void whenHeldTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + InternalButton button = new InternalButton(); + button.setPressed(false); + button.whenHeld(command1); + scheduler.run(); + verify(command1, never()).schedule(true); + button.setPressed(true); + scheduler.run(); + scheduler.run(); + verify(command1).schedule(true); + button.setPressed(false); + scheduler.run(); + verify(command1).cancel(); + } + + @Test + void toggleWhenPressedTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + InternalButton button = new InternalButton(); + button.setPressed(false); + button.toggleWhenPressed(command1); + scheduler.run(); + verify(command1, never()).schedule(true); + button.setPressed(true); + scheduler.run(); + when(command1.isScheduled()).thenReturn(true); + scheduler.run(); + verify(command1).schedule(true); + button.setPressed(false); + scheduler.run(); + verify(command1, never()).cancel(); + button.setPressed(true); + scheduler.run(); + verify(command1).cancel(); + } + + @Test + void cancelWhenPressedTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + InternalButton button = new InternalButton(); + button.setPressed(false); + button.cancelWhenPressed(command1); + scheduler.run(); + verify(command1, never()).cancel(); + button.setPressed(true); + scheduler.run(); + scheduler.run(); + verify(command1).cancel(); + } + + @Test + void runnableBindingTest() { + + InternalButton buttonWhenPressed = new InternalButton(); + InternalButton buttonWhileHeld = new InternalButton(); + InternalButton buttonWhenReleased = new InternalButton(); + + buttonWhenPressed.setPressed(false); + buttonWhileHeld.setPressed(true); + buttonWhenReleased.setPressed(true); + + Counter counter = new Counter(); + + buttonWhenPressed.whenPressed(counter::increment); + buttonWhileHeld.whileHeld(counter::increment); + buttonWhenReleased.whenReleased(counter::increment); + + CommandScheduler scheduler = CommandScheduler.getInstance(); + + scheduler.run(); + buttonWhenPressed.setPressed(true); + buttonWhenReleased.setPressed(false); + scheduler.run(); + + assertEquals(counter.m_counter, 4); + } + + @Test + void buttonCompositionTest() { + InternalButton button1 = new InternalButton(); + InternalButton button2 = new InternalButton(); + + button1.setPressed(true); + button2.setPressed(false); + + assertFalse(button1.and(button2).get()); + assertTrue(button1.or(button2).get()); + assertFalse(button1.negate().get()); + assertTrue(button1.and(button2.negate()).get()); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java new file mode 100644 index 0000000000..cfe0e9f8d0 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java @@ -0,0 +1,182 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.Timer; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class CommandDecoratorTest extends CommandTestBase { + @Test + void withTimeoutTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Command command1 = new WaitCommand(10); + + Command timeout = command1.withTimeout(2); + + scheduler.schedule(timeout); + scheduler.run(); + + assertFalse(scheduler.isScheduled(command1)); + assertTrue(scheduler.isScheduled(timeout)); + + Timer.delay(3); + scheduler.run(); + + assertFalse(scheduler.isScheduled(timeout)); + } + + @Test + void interruptOnTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + + Command command = new WaitCommand(10).interruptOn(condition::getCondition); + + scheduler.schedule(command); + scheduler.run(); + assertTrue(scheduler.isScheduled(command)); + condition.setCondition(true); + scheduler.run(); + assertFalse(scheduler.isScheduled(command)); + } + + @Test + void beforeStartingTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + condition.setCondition(false); + + Command command = new InstantCommand(); + + scheduler.schedule(command.beforeStarting(() -> condition.setCondition(true))); + + assertTrue(condition.getCondition()); + } + + @Test + void whenFinishedTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + condition.setCondition(false); + + Command command = new InstantCommand(); + + scheduler.schedule(command.whenFinished(() -> condition.setCondition(true))); + + assertFalse(condition.getCondition()); + + scheduler.run(); + + assertTrue(condition.getCondition()); + } + + @Test + void andThenTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + condition.setCondition(false); + + Command command1 = new InstantCommand(); + Command command2 = new InstantCommand(() -> condition.setCondition(true)); + + scheduler.schedule(command1.andThen(command2)); + + assertFalse(condition.getCondition()); + + scheduler.run(); + + assertTrue(condition.getCondition()); + } + + @Test + void deadlineWithTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + condition.setCondition(false); + + Command dictator = new WaitUntilCommand(condition::getCondition); + Command endsBefore = new InstantCommand(); + Command endsAfter = new WaitUntilCommand(() -> false); + + Command group = dictator.deadlineWith(endsBefore, endsAfter); + + scheduler.schedule(group); + scheduler.run(); + + assertTrue(scheduler.isScheduled(group)); + + condition.setCondition(true); + scheduler.run(); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void alongWithTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + condition.setCondition(false); + + Command command1 = new WaitUntilCommand(condition::getCondition); + Command command2 = new InstantCommand(); + + Command group = command1.alongWith(command2); + + scheduler.schedule(group); + scheduler.run(); + + assertTrue(scheduler.isScheduled(group)); + + condition.setCondition(true); + scheduler.run(); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void raceWithTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Command command1 = new WaitUntilCommand(() -> false); + Command command2 = new InstantCommand(); + + Command group = command1.raceWith(command2); + + scheduler.schedule(group); + scheduler.run(); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void perpetuallyTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Command command = new InstantCommand(); + + Command perpetual = command.perpetually(); + + scheduler.schedule(perpetual); + scheduler.run(); + scheduler.run(); + scheduler.run(); + + assertTrue(scheduler.isScheduled(perpetual)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java new file mode 100644 index 0000000000..b40f172754 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertThrows; + +class CommandGroupErrorTest extends CommandTestBase { + @Test + void commandInMultipleGroupsTest() { + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + @SuppressWarnings("PMD.UnusedLocalVariable") + Command group = new ParallelCommandGroup(command1, command2); + assertThrows(IllegalArgumentException.class, + () -> new ParallelCommandGroup(command1, command2)); + } + + @Test + void commandInGroupExternallyScheduledTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + @SuppressWarnings("PMD.UnusedLocalVariable") + Command group = new ParallelCommandGroup(command1, command2); + + assertThrows(IllegalArgumentException.class, + () -> scheduler.schedule(command1)); + } + + @Test + void redecoratedCommandErrorTest() { + Command command = new InstantCommand(); + + assertDoesNotThrow(() -> command.withTimeout(10).interruptOn(() -> false)); + assertThrows(IllegalArgumentException.class, () -> command.withTimeout(10)); + CommandGroupBase.clearGroupedCommand(command); + assertDoesNotThrow(() -> command.withTimeout(10)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java new file mode 100644 index 0000000000..10d3ee06a7 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.verify; + +@SuppressWarnings("PMD.TooManyMethods") +class CommandRequirementsTest extends CommandTestBase { + @Test + void requirementInterruptTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Subsystem requirement = new TestSubsystem(); + + MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement); + Command interrupted = interruptedHolder.getMock(); + MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement); + Command interrupter = interrupterHolder.getMock(); + + scheduler.schedule(interrupted); + scheduler.run(); + scheduler.schedule(interrupter); + scheduler.run(); + + verify(interrupted).initialize(); + verify(interrupted).execute(); + verify(interrupted).end(true); + + verify(interrupter).initialize(); + verify(interrupter).execute(); + + assertFalse(scheduler.isScheduled(interrupted)); + assertTrue(scheduler.isScheduled(interrupter)); + } + + @Test + void requirementUninterruptibleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Subsystem requirement = new TestSubsystem(); + + MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement); + Command notInterrupted = interruptedHolder.getMock(); + MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement); + Command interrupter = interrupterHolder.getMock(); + + scheduler.schedule(false, notInterrupted); + scheduler.schedule(interrupter); + + assertTrue(scheduler.isScheduled(notInterrupted)); + assertFalse(scheduler.isScheduled(interrupter)); + } + + @Test + void defaultCommandRequirementErrorTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Subsystem system = new TestSubsystem(); + + Command missingRequirement = new WaitUntilCommand(() -> false); + Command ends = new InstantCommand(() -> { + }, system); + + assertThrows(IllegalArgumentException.class, + () -> scheduler.setDefaultCommand(system, missingRequirement)); + assertThrows(IllegalArgumentException.class, + () -> scheduler.setDefaultCommand(system, ends)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java new file mode 100644 index 0000000000..aee30db89e --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java @@ -0,0 +1,122 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.times; +import static org.mockito.Mockito.verify; + +class CommandScheduleTest extends CommandTestBase { + @Test + void instantScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder holder = new MockCommandHolder(true); + holder.setFinished(true); + Command mockCommand = holder.getMock(); + + scheduler.schedule(mockCommand); + assertTrue(scheduler.isScheduled(mockCommand)); + verify(mockCommand).initialize(); + + scheduler.run(); + + verify(mockCommand).execute(); + verify(mockCommand).end(false); + + assertFalse(scheduler.isScheduled(mockCommand)); + } + + @Test + void singleIterationScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder holder = new MockCommandHolder(true); + Command mockCommand = holder.getMock(); + + scheduler.schedule(mockCommand); + + assertTrue(scheduler.isScheduled(mockCommand)); + + scheduler.run(); + holder.setFinished(true); + scheduler.run(); + + verify(mockCommand).initialize(); + verify(mockCommand, times(2)).execute(); + verify(mockCommand).end(false); + + assertFalse(scheduler.isScheduled(mockCommand)); + } + + @Test + void multiScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + + scheduler.schedule(true, command1, command2, command3); + assertTrue(scheduler.isScheduled(command1, command2, command3)); + scheduler.run(); + assertTrue(scheduler.isScheduled(command1, command2, command3)); + + command1Holder.setFinished(true); + scheduler.run(); + assertTrue(scheduler.isScheduled(command2, command3)); + assertFalse(scheduler.isScheduled(command1)); + + command2Holder.setFinished(true); + scheduler.run(); + assertTrue(scheduler.isScheduled(command3)); + assertFalse(scheduler.isScheduled(command1, command2)); + + command3Holder.setFinished(true); + scheduler.run(); + assertFalse(scheduler.isScheduled(command1, command2, command3)); + } + + @Test + void schedulerCancelTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder holder = new MockCommandHolder(true); + Command mockCommand = holder.getMock(); + + scheduler.schedule(mockCommand); + + scheduler.run(); + scheduler.cancel(mockCommand); + scheduler.run(); + + verify(mockCommand).execute(); + verify(mockCommand).end(true); + verify(mockCommand, never()).end(false); + + assertFalse(scheduler.isScheduled(mockCommand)); + } + + @Test + void notScheduledCancelTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder holder = new MockCommandHolder(true); + Command mockCommand = holder.getMock(); + + assertDoesNotThrow(() -> scheduler.cancel(mockCommand)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java new file mode 100644 index 0000000000..61aa1a07a3 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java @@ -0,0 +1,92 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Set; + +import org.junit.jupiter.api.BeforeEach; + +import edu.wpi.first.hal.sim.DriverStationSim; +import edu.wpi.first.wpilibj.DriverStation; + +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.when; + +/** + * Basic setup for all {@link Command tests}." + */ +@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod") +abstract class CommandTestBase { + @BeforeEach + void commandSetup() { + CommandScheduler.getInstance().cancelAll(); + CommandScheduler.getInstance().enable(); + CommandScheduler.getInstance().clearButtons(); + CommandGroupBase.clearGroupedCommands(); + + setDSEnabled(true); + } + + void setDSEnabled(boolean enabled) { + DriverStationSim sim = new DriverStationSim(); + sim.setDsAttached(true); + + sim.setEnabled(enabled); + sim.notifyNewData(); + DriverStation.getInstance().isNewControlData(); + while (DriverStation.getInstance().isEnabled() != enabled) { + try { + Thread.sleep(1); + } catch (InterruptedException exception) { + exception.printStackTrace(); + } + } + } + + class TestSubsystem extends SubsystemBase { + } + + protected class MockCommandHolder { + private final Command m_mockCommand = mock(Command.class); + + MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) { + when(m_mockCommand.getRequirements()).thenReturn(Set.of(requirements)); + when(m_mockCommand.isFinished()).thenReturn(false); + when(m_mockCommand.runsWhenDisabled()).thenReturn(runWhenDisabled); + } + + Command getMock() { + return m_mockCommand; + } + + void setFinished(boolean finished) { + when(m_mockCommand.isFinished()).thenReturn(finished); + } + + } + + protected class Counter { + int m_counter; + + void increment() { + m_counter++; + } + } + + protected class ConditionHolder { + private boolean m_condition; + + void setCondition(boolean condition) { + m_condition = condition; + } + + boolean getCondition() { + return m_condition; + } + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java new file mode 100644 index 0000000000..0740565b71 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java @@ -0,0 +1,65 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.verify; + +class ConditionalCommandTest extends CommandTestBase { + @Test + void conditionalCommandTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + command1Holder.setFinished(true); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true); + + scheduler.schedule(conditionalCommand); + scheduler.run(); + + verify(command1).initialize(); + verify(command1).execute(); + verify(command1).end(false); + + verify(command2, never()).initialize(); + verify(command2, never()).execute(); + verify(command2, never()).end(false); + } + + @Test + void conditionalCommandRequirementTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system3); + Command command2 = command2Holder.getMock(); + + ConditionalCommand conditionalCommand = new ConditionalCommand(command1, command2, () -> true); + + scheduler.schedule(conditionalCommand); + scheduler.schedule(new InstantCommand(() -> { + }, system3)); + + assertFalse(scheduler.isScheduled(conditionalCommand)); + + verify(command1).end(true); + verify(command2, never()).end(true); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java new file mode 100644 index 0000000000..033dcc5e43 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.verify; + +class DefaultCommandTest extends CommandTestBase { + @Test + void defaultCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + TestSubsystem hasDefaultCommand = new TestSubsystem(); + + MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand); + Command defaultCommand = defaultHolder.getMock(); + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand); + scheduler.run(); + + assertTrue(scheduler.isScheduled(defaultCommand)); + } + + @Test + void defaultCommandInterruptResumeTest() { + CommandScheduler scheduler = new CommandScheduler(); + + TestSubsystem hasDefaultCommand = new TestSubsystem(); + + MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand); + Command defaultCommand = defaultHolder.getMock(); + MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand); + Command interrupter = interrupterHolder.getMock(); + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand); + scheduler.run(); + scheduler.schedule(interrupter); + + assertFalse(scheduler.isScheduled(defaultCommand)); + assertTrue(scheduler.isScheduled(interrupter)); + + scheduler.cancel(interrupter); + scheduler.run(); + + assertTrue(scheduler.isScheduled(defaultCommand)); + assertFalse(scheduler.isScheduled(interrupter)); + } + + @Test + void defaultCommandDisableResumeTest() { + CommandScheduler scheduler = new CommandScheduler(); + + TestSubsystem hasDefaultCommand = new TestSubsystem(); + + MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand); + Command defaultCommand = defaultHolder.getMock(); + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand); + scheduler.run(); + + assertTrue(scheduler.isScheduled(defaultCommand)); + + setDSEnabled(false); + scheduler.run(); + + assertFalse(scheduler.isScheduled(defaultCommand)); + + setDSEnabled(true); + scheduler.run(); + + assertTrue(scheduler.isScheduled(defaultCommand)); + + verify(defaultCommand).end(true); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java new file mode 100644 index 0000000000..23f508a339 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class FunctionalCommandTest extends CommandTestBase { + @Test + void functionalCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder cond1 = new ConditionHolder(); + ConditionHolder cond2 = new ConditionHolder(); + ConditionHolder cond3 = new ConditionHolder(); + ConditionHolder cond4 = new ConditionHolder(); + + FunctionalCommand command = + new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true), + interrupted -> cond3.setCondition(true), cond4::getCondition); + + scheduler.schedule(command); + scheduler.run(); + + assertTrue(scheduler.isScheduled(command)); + + cond4.setCondition(true); + + scheduler.run(); + + assertFalse(scheduler.isScheduled(command)); + assertTrue(cond1.getCondition()); + assertTrue(cond2.getCondition()); + assertTrue(cond3.getCondition()); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java new file mode 100644 index 0000000000..a109ed65c6 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class InstantCommandTest extends CommandTestBase { + @Test + void instantCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder cond = new ConditionHolder(); + + InstantCommand command = new InstantCommand(() -> cond.setCondition(true)); + + scheduler.schedule(command); + scheduler.run(); + + assertTrue(cond.getCondition()); + assertFalse(scheduler.isScheduled(command)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java new file mode 100644 index 0000000000..1bed736f18 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.condition.DisabledOnOs; +import org.junit.jupiter.api.condition.OS; + +import edu.wpi.first.wpilibj.Timer; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +@DisabledOnOs(OS.MAC) +class NotifierCommandTest extends CommandTestBase { + @Test + void notifierCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Counter counter = new Counter(); + + NotifierCommand command = new NotifierCommand(counter::increment, .01); + + scheduler.schedule(command); + Timer.delay(.25); + scheduler.cancel(command); + + assertEquals(.25, 0.01 * counter.m_counter, .025); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java new file mode 100644 index 0000000000..d03b4aa4dd --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java @@ -0,0 +1,131 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.times; +import static org.mockito.Mockito.verify; + +class ParallelCommandGroupTest extends CommandTestBase { + @Test + void parallelGroupScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new ParallelCommandGroup(command1, command2); + + scheduler.schedule(group); + + verify(command1).initialize(); + verify(command2).initialize(); + + command1Holder.setFinished(true); + scheduler.run(); + command2Holder.setFinished(true); + scheduler.run(); + + verify(command1).execute(); + verify(command1).end(false); + verify(command2, times(2)).execute(); + verify(command2).end(false); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void parallelGroupInterruptTest() { + CommandScheduler scheduler = new CommandScheduler(); + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new ParallelCommandGroup(command1, command2); + + scheduler.schedule(group); + + command1Holder.setFinished(true); + scheduler.run(); + scheduler.run(); + scheduler.cancel(group); + + verify(command1).execute(); + verify(command1).end(false); + verify(command1, never()).end(true); + + verify(command2, times(2)).execute(); + verify(command2, never()).end(false); + verify(command2).end(true); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void notScheduledCancelTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new ParallelCommandGroup(command1, command2); + + assertDoesNotThrow(() -> scheduler.cancel(group)); + } + + @Test + void parallelGroupRequirementTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + Subsystem system4 = new TestSubsystem(); + + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system3); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4); + Command command3 = command3Holder.getMock(); + + Command group = new ParallelCommandGroup(command1, command2); + + scheduler.schedule(group); + scheduler.schedule(command3); + + assertFalse(scheduler.isScheduled(group)); + assertTrue(scheduler.isScheduled(command3)); + } + + @Test + void parallelGroupRequirementErrorTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3); + Command command2 = command2Holder.getMock(); + + assertThrows(IllegalArgumentException.class, + () -> new ParallelCommandGroup(command1, command2)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java new file mode 100644 index 0000000000..c7e3769f51 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java @@ -0,0 +1,129 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.verify; +import static org.mockito.internal.verification.VerificationModeFactory.times; + +class ParallelDeadlineGroupTest extends CommandTestBase { + @Test + void parallelDeadlineScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + command2Holder.setFinished(true); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + + Command group = new ParallelDeadlineGroup(command1, command2, command3); + + scheduler.schedule(group); + scheduler.run(); + + assertTrue(scheduler.isScheduled(group)); + + command1Holder.setFinished(true); + scheduler.run(); + + assertFalse(scheduler.isScheduled(group)); + + verify(command2).initialize(); + verify(command2).execute(); + verify(command2).end(false); + verify(command2, never()).end(true); + + verify(command1).initialize(); + verify(command1, times(2)).execute(); + verify(command1).end(false); + verify(command1, never()).end(true); + + verify(command3).initialize(); + verify(command3, times(2)).execute(); + verify(command3, never()).end(false); + verify(command3).end(true); + } + + @Test + void parallelDeadlineInterruptTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + command2Holder.setFinished(true); + + Command group = new ParallelDeadlineGroup(command1, command2); + + scheduler.schedule(group); + + scheduler.run(); + scheduler.run(); + scheduler.cancel(group); + + verify(command1, times(2)).execute(); + verify(command1, never()).end(false); + verify(command1).end(true); + + verify(command2).execute(); + verify(command2).end(false); + verify(command2, never()).end(true); + + assertFalse(scheduler.isScheduled(group)); + } + + + @Test + void parallelDeadlineRequirementTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + Subsystem system4 = new TestSubsystem(); + + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system3); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4); + Command command3 = command3Holder.getMock(); + + Command group = new ParallelDeadlineGroup(command1, command2); + + scheduler.schedule(group); + scheduler.schedule(command3); + + assertFalse(scheduler.isScheduled(group)); + assertTrue(scheduler.isScheduled(command3)); + } + + @Test + void parallelDeadlineRequirementErrorTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3); + Command command2 = command2Holder.getMock(); + + assertThrows(IllegalArgumentException.class, + () -> new ParallelDeadlineGroup(command1, command2)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java new file mode 100644 index 0000000000..c9b21e4bad --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java @@ -0,0 +1,132 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.times; +import static org.mockito.Mockito.verify; + +class ParallelRaceGroupTest extends CommandTestBase { + @Test + void parallelRaceScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new ParallelRaceGroup(command1, command2); + + scheduler.schedule(group); + + verify(command1).initialize(); + verify(command2).initialize(); + + command1Holder.setFinished(true); + scheduler.run(); + command2Holder.setFinished(true); + scheduler.run(); + + verify(command1).execute(); + verify(command1).end(false); + verify(command2).execute(); + verify(command2).end(true); + verify(command2, never()).end(false); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void parallelRaceInterruptTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new ParallelRaceGroup(command1, command2); + + scheduler.schedule(group); + + scheduler.run(); + scheduler.run(); + scheduler.cancel(group); + + verify(command1, times(2)).execute(); + verify(command1, never()).end(false); + verify(command1).end(true); + + verify(command2, times(2)).execute(); + verify(command2, never()).end(false); + verify(command2).end(true); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void notScheduledCancelTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new ParallelRaceGroup(command1, command2); + + assertDoesNotThrow(() -> scheduler.cancel(group)); + } + + + @Test + void parallelRaceRequirementTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + Subsystem system4 = new TestSubsystem(); + + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system3); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4); + Command command3 = command3Holder.getMock(); + + Command group = new ParallelRaceGroup(command1, command2); + + scheduler.schedule(group); + scheduler.schedule(command3); + + assertFalse(scheduler.isScheduled(group)); + assertTrue(scheduler.isScheduled(command3)); + } + + @Test + void parallelRaceRequirementErrorTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3); + Command command2 = command2Holder.getMock(); + + assertThrows(IllegalArgumentException.class, () -> new ParallelRaceGroup(command1, command2)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java new file mode 100644 index 0000000000..baf037f5cc --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +class PerpetualCommandTest extends CommandTestBase { + @Test + void perpetualCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + PerpetualCommand command = new PerpetualCommand(new InstantCommand()); + + scheduler.schedule(command); + scheduler.run(); + + assertTrue(scheduler.isScheduled(command)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java new file mode 100644 index 0000000000..7484396268 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.io.ByteArrayOutputStream; +import java.io.PrintStream; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; + +class PrintCommandTest extends CommandTestBase { + @Test + void printCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + final PrintStream originalOut = System.out; + ByteArrayOutputStream testOut = new ByteArrayOutputStream(); + System.setOut(new PrintStream(testOut)); + + PrintCommand command = new PrintCommand("Test!"); + + scheduler.schedule(command); + scheduler.run(); + + assertFalse(scheduler.isScheduled(command)); + assertEquals(testOut.toString(), "Test!" + System.lineSeparator()); + + System.setOut(originalOut); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java new file mode 100644 index 0000000000..b566aae568 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.verify; + +class ProxyScheduleCommandTest extends CommandTestBase { + @Test + void proxyScheduleCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + + ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command1); + + scheduler.schedule(scheduleCommand); + + verify(command1).schedule(); + } + + @Test + void proxyScheduleCommandEndTest() { + CommandScheduler scheduler = CommandScheduler.getInstance(); + + ConditionHolder cond = new ConditionHolder(); + + WaitUntilCommand command = new WaitUntilCommand(cond::getCondition); + + ProxyScheduleCommand scheduleCommand = new ProxyScheduleCommand(command); + + scheduler.schedule(scheduleCommand); + + scheduler.run(); + assertTrue(scheduler.isScheduled(scheduleCommand)); + + cond.setCondition(true); + scheduler.run(); + scheduler.run(); + assertFalse(scheduler.isScheduled(scheduleCommand)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java new file mode 100644 index 0000000000..e862216c5e --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java @@ -0,0 +1,183 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Map; + +import org.junit.jupiter.api.Test; + +import static edu.wpi.first.wpilibj2.command.CommandGroupBase.parallel; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class RobotDisabledCommandTest extends CommandTestBase { + @Test + void robotDisabledCommandCancelTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder holder = new MockCommandHolder(false); + Command mockCommand = holder.getMock(); + + scheduler.schedule(mockCommand); + + assertTrue(scheduler.isScheduled(mockCommand)); + + setDSEnabled(false); + + scheduler.run(); + + assertFalse(scheduler.isScheduled(mockCommand)); + + setDSEnabled(true); + } + + @Test + void runWhenDisabledTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder holder = new MockCommandHolder(true); + Command mockCommand = holder.getMock(); + + scheduler.schedule(mockCommand); + + assertTrue(scheduler.isScheduled(mockCommand)); + + setDSEnabled(false); + + scheduler.run(); + + assertTrue(scheduler.isScheduled(mockCommand)); + } + + @Test + void sequentialGroupRunWhenDisabledTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + MockCommandHolder command4Holder = new MockCommandHolder(false); + Command command4 = command4Holder.getMock(); + + Command runWhenDisabled = new SequentialCommandGroup(command1, command2); + Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4); + + scheduler.schedule(runWhenDisabled); + scheduler.schedule(dontRunWhenDisabled); + + setDSEnabled(false); + + scheduler.run(); + + assertTrue(scheduler.isScheduled(runWhenDisabled)); + assertFalse(scheduler.isScheduled(dontRunWhenDisabled)); + } + + @Test + void parallelGroupRunWhenDisabledTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + MockCommandHolder command4Holder = new MockCommandHolder(false); + Command command4 = command4Holder.getMock(); + + Command runWhenDisabled = new ParallelCommandGroup(command1, command2); + Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4); + + scheduler.schedule(runWhenDisabled); + scheduler.schedule(dontRunWhenDisabled); + + setDSEnabled(false); + + scheduler.run(); + + assertTrue(scheduler.isScheduled(runWhenDisabled)); + assertFalse(scheduler.isScheduled(dontRunWhenDisabled)); + } + + @Test + void conditionalRunWhenDisabledTest() { + setDSEnabled(false); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + MockCommandHolder command4Holder = new MockCommandHolder(false); + Command command4 = command4Holder.getMock(); + + CommandScheduler scheduler = new CommandScheduler(); + + Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true); + Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true); + + scheduler.schedule(runWhenDisabled, dontRunWhenDisabled); + + assertTrue(scheduler.isScheduled(runWhenDisabled)); + assertFalse(scheduler.isScheduled(dontRunWhenDisabled)); + } + + @Test + void selectRunWhenDisabledTest() { + setDSEnabled(false); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + MockCommandHolder command4Holder = new MockCommandHolder(false); + Command command4 = command4Holder.getMock(); + + Command runWhenDisabled = new SelectCommand(Map.of(1, command1, 2, command2), () -> 1); + Command dontRunWhenDisabled = new SelectCommand(Map.of(1, command3, 2, command4), () -> 1); + + CommandScheduler scheduler = new CommandScheduler(); + + scheduler.schedule(runWhenDisabled, dontRunWhenDisabled); + + assertTrue(scheduler.isScheduled(runWhenDisabled)); + assertFalse(scheduler.isScheduled(dontRunWhenDisabled)); + } + + @Test + void parallelConditionalRunWhenDisabledTest() { + setDSEnabled(false); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + MockCommandHolder command4Holder = new MockCommandHolder(false); + Command command4 = command4Holder.getMock(); + + CommandScheduler scheduler = new CommandScheduler(); + + Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true); + Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true); + + Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled); + + scheduler.schedule(parallel); + + assertFalse(scheduler.isScheduled(runWhenDisabled)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java new file mode 100644 index 0000000000..3ff8c3dfdf --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class RunCommandTest extends CommandTestBase { + @Test + void runCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Counter counter = new Counter(); + + RunCommand command = new RunCommand(counter::increment); + + scheduler.schedule(command); + scheduler.run(); + scheduler.run(); + scheduler.run(); + + assertEquals(3, counter.m_counter); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java new file mode 100644 index 0000000000..0a5035233b --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.mockito.Mockito.verify; + +class ScheduleCommandTest extends CommandTestBase { + @Test + void scheduleCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2); + + scheduler.schedule(scheduleCommand); + + verify(command1).schedule(); + verify(command2).schedule(); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java new file mode 100644 index 0000000000..1f110b68ed --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + +class SchedulerTest extends CommandTestBase { + @Test + void schedulerLambdaTestNoInterrupt() { + CommandScheduler scheduler = new CommandScheduler(); + + Counter counter = new Counter(); + + scheduler.onCommandInitialize(command -> counter.increment()); + scheduler.onCommandExecute(command -> counter.increment()); + scheduler.onCommandFinish(command -> counter.increment()); + + scheduler.schedule(new InstantCommand()); + scheduler.run(); + + assertEquals(counter.m_counter, 3); + } + + @Test + void schedulerInterruptLambdaTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Counter counter = new Counter(); + + scheduler.onCommandInterrupt(command -> counter.increment()); + + Command command = new WaitCommand(10); + + scheduler.schedule(command); + scheduler.cancel(command); + + assertEquals(counter.m_counter, 1); + } + + @Test + void unregisterSubsystemTest() { + CommandScheduler scheduler = new CommandScheduler(); + + Subsystem system = new TestSubsystem(); + + scheduler.registerSubsystem(system); + assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java new file mode 100644 index 0000000000..df52855f46 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java @@ -0,0 +1,108 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import java.util.Map; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.verify; + +class SelectCommandTest extends CommandTestBase { + @Test + void selectCommandTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + command1Holder.setFinished(true); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + + SelectCommand selectCommand = + new SelectCommand(Map.ofEntries( + Map.entry("one", command1), + Map.entry("two", command2), + Map.entry("three", command3)), + () -> "one"); + + scheduler.schedule(selectCommand); + scheduler.run(); + + verify(command1).initialize(); + verify(command1).execute(); + verify(command1).end(false); + + verify(command2, never()).initialize(); + verify(command2, never()).execute(); + verify(command2, never()).end(false); + + verify(command3, never()).initialize(); + verify(command3, never()).execute(); + verify(command3, never()).end(false); + } + + @Test + void selectCommandInvalidKeyTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + command1Holder.setFinished(true); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + + SelectCommand selectCommand = + new SelectCommand(Map.ofEntries( + Map.entry("one", command1), + Map.entry("two", command2), + Map.entry("three", command3)), + () -> "four"); + + assertDoesNotThrow(() -> scheduler.schedule(selectCommand)); + } + + + @Test + void selectCommandRequirementTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + Subsystem system4 = new TestSubsystem(); + + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system3); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4); + Command command3 = command3Holder.getMock(); + + SelectCommand selectCommand = new SelectCommand( + Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2), + Map.entry("three", command3)), () -> "one"); + + scheduler.schedule(selectCommand); + scheduler.schedule(new InstantCommand(() -> { + }, system3)); + + assertFalse(scheduler.isScheduled(selectCommand)); + + verify(command1).end(true); + verify(command2, never()).end(true); + verify(command3, never()).end(true); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java new file mode 100644 index 0000000000..8582140547 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java @@ -0,0 +1,128 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.verify; + +class SequentialCommandGroupTest extends CommandTestBase { + @Test + void sequentialGroupScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new SequentialCommandGroup(command1, command2); + + scheduler.schedule(group); + + verify(command1).initialize(); + verify(command2, never()).initialize(); + + command1Holder.setFinished(true); + scheduler.run(); + + verify(command1).execute(); + verify(command1).end(false); + verify(command2).initialize(); + verify(command2, never()).execute(); + verify(command2, never()).end(false); + + command2Holder.setFinished(true); + scheduler.run(); + + verify(command1).execute(); + verify(command1).end(false); + verify(command2).execute(); + verify(command2).end(false); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void sequentialGroupInterruptTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true); + Command command3 = command3Holder.getMock(); + + Command group = new SequentialCommandGroup(command1, command2, command3); + + scheduler.schedule(group); + + command1Holder.setFinished(true); + scheduler.run(); + scheduler.cancel(group); + scheduler.run(); + + verify(command1).execute(); + verify(command1, never()).end(true); + verify(command1).end(false); + verify(command2, never()).execute(); + verify(command2).end(true); + verify(command2, never()).end(false); + verify(command3, never()).initialize(); + verify(command3, never()).execute(); + verify(command3, never()).end(true); + verify(command3, never()).end(false); + + assertFalse(scheduler.isScheduled(group)); + } + + @Test + void notScheduledCancelTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true); + Command command2 = command2Holder.getMock(); + + Command group = new SequentialCommandGroup(command1, command2); + + assertDoesNotThrow(() -> scheduler.cancel(group)); + } + + + @Test + void sequentialGroupRequirementTest() { + Subsystem system1 = new TestSubsystem(); + Subsystem system2 = new TestSubsystem(); + Subsystem system3 = new TestSubsystem(); + Subsystem system4 = new TestSubsystem(); + + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2); + Command command1 = command1Holder.getMock(); + MockCommandHolder command2Holder = new MockCommandHolder(true, system3); + Command command2 = command2Holder.getMock(); + MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4); + Command command3 = command3Holder.getMock(); + + Command group = new SequentialCommandGroup(command1, command2); + + scheduler.schedule(group); + scheduler.schedule(command3); + + assertFalse(scheduler.isScheduled(group)); + assertTrue(scheduler.isScheduled(command3)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java new file mode 100644 index 0000000000..93921e04e2 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class StartEndCommandTest extends CommandTestBase { + @Test + void startEndCommandScheduleTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder cond1 = new ConditionHolder(); + ConditionHolder cond2 = new ConditionHolder(); + + StartEndCommand command = + new StartEndCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true)); + + scheduler.schedule(command); + scheduler.run(); + + assertTrue(scheduler.isScheduled(command)); + + scheduler.cancel(command); + + assertFalse(scheduler.isScheduled(command)); + assertTrue(cond1.getCondition()); + assertTrue(cond2.getCondition()); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java new file mode 100644 index 0000000000..5a522af28d --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.Timer; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.ArgumentMatchers.anyDouble; +import static org.mockito.Mockito.never; +import static org.mockito.Mockito.verify; +import static org.mockito.Mockito.when; + +class WaitCommandTest extends CommandTestBase { + @Test + void waitCommandTest() { + CommandScheduler scheduler = new CommandScheduler(); + + WaitCommand waitCommand = new WaitCommand(2); + + scheduler.schedule(waitCommand); + scheduler.run(); + Timer.delay(1); + scheduler.run(); + + assertTrue(scheduler.isScheduled(waitCommand)); + + Timer.delay(2); + + scheduler.run(); + + assertFalse(scheduler.isScheduled(waitCommand)); + } + + @Test + void withTimeoutTest() { + CommandScheduler scheduler = new CommandScheduler(); + + MockCommandHolder command1Holder = new MockCommandHolder(true); + Command command1 = command1Holder.getMock(); + when(command1.withTimeout(anyDouble())).thenCallRealMethod(); + + Command timeout = command1.withTimeout(2); + + scheduler.schedule(timeout); + scheduler.run(); + + verify(command1).initialize(); + verify(command1).execute(); + assertFalse(scheduler.isScheduled(command1)); + assertTrue(scheduler.isScheduled(timeout)); + + Timer.delay(3); + scheduler.run(); + + verify(command1).end(true); + verify(command1, never()).end(false); + assertFalse(scheduler.isScheduled(timeout)); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java new file mode 100644 index 0000000000..a99a18cac5 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj2.command; + +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class WaitUntilCommandTest extends CommandTestBase { + @Test + void waitUntilTest() { + CommandScheduler scheduler = new CommandScheduler(); + + ConditionHolder condition = new ConditionHolder(); + + Command command = new WaitUntilCommand(condition::getCondition); + + scheduler.schedule(command); + scheduler.run(); + assertTrue(scheduler.isScheduled(command)); + condition.setCondition(true); + scheduler.run(); + assertFalse(scheduler.isScheduled(command)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index a383c396cb..c78854e64f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -230,6 +230,16 @@ "gradlebase": "java", "mainclass": "Main" }, + { + "name": "GearsBot (New)", + "description": "A fully functional example CommandBased program for WPIs GearsBot robot, ported to the new CommandBased library. This code can run on your computer if it supports simulation.", + "tags": [ + "Complete Robot" + ], + "foldername": "gearsbotnew", + "gradlebase": "java", + "mainclass": "Main" + }, { "name": "PacGoat", "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.", @@ -282,5 +292,66 @@ "foldername": "shuffleboard", "gradlebase": "java", "mainclass": "Main" + }, + { + "name": "'Traditional' Hatchbot", + "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'traditional' style, i.e. commands are given their own classes.", + "tags": [ + "Complete robot", + "Command-based" + ], + "foldername": "hatchbottraditional", + "mainclass": "Main" + }, + { + "name": "'Inlined' Hatchbot", + "description": "A fully-functional command-based hatchbot for the 2019 game using the new experimental command API. Written in the 'inlined' style, i.e. many commands are defined inline with lambdas.", + "tags": [ + "Complete robot", + "Command-based", + "Lambdas" + ], + "foldername": "hatchbotinlined", + "mainclass": "Main" + }, + { + "name": "Select Command Example", + "description": "An example showing how to use the SelectCommand class from the experimental command framework rewrite.", + "tags": [ + "Command-based" + ], + "foldername": "selectcommand", + "mainclass": "Main" + }, + { + "name": "Scheduler Event Logging", + "description": "An example showing how to use Shuffleboard to log Command events from the CommandScheduler in the experimental command framework rewrite", + "tags": [ + "Command-based", + "Shuffleboard" + ], + "foldername": "schedulereventlogging", + "mainclass": "Main" + }, + { + "name": "Frisbeebot", + "description": "An example robot project for a simple frisbee shooter for the 2013 FRC game, Ultimate Ascent, demonstrating use of PID functionality in the command framework", + "tags": [ + "Command-based", + "PID" + ], + "foldername": "frisbeebot", + "mainclass": "Main" + }, + { + "name": "Gyro Drive Commands", + "description": "An example command-based robot project demonstrating simple PID functionality utilizing a gyroscope to keep a robot driving straight and to turn to specified angles.", + "tags": [ + "Command-based", + "PID", + "Gyro" + ], + "foldername": "gyrodrivecommands", + "mainclass": "Main" } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java new file mode 100644 index 0000000000..384fb9892f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.frisbeebot; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterInches = 6; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR; + } + + public static final class ShooterConstants { + public static final int[] kEncoderPorts = new int[]{4, 5}; + public static final boolean kEncoderReversed = false; + public static final int kEncoderCPR = 1024; + public static final double kEncoderDistancePerPulse = + // Distance units will be rotations + 1. / (double) kEncoderCPR; + + public static final int kShooterMotorPort = 4; + public static final int kFeederMotorPort = 5; + + public static final double kShooterFreeRPS = 5300; + public static final double kShooterTargetRPS = 4000; + public static final double kShooterToleranceRPS = 50; + + public static final double kP = 1; + public static final double kI = 0; + public static final double kD = 0; + + // On a real robot the feedforward constants should be empirically determined; these are + // reasonable guesses. + public static final double kSFractional = .05; + public static final double kVFractional = + // Should have value 1 at free speed... + 1. / kShooterFreeRPS; + + public static final double kFeederSpeed = .5; + } + + public static final class AutoConstants { + public static final double kAutoTimeoutSeconds = 12; + public static final double kAutoShootTimeSeconds = 7; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java new file mode 100644 index 0000000000..1fdb348e5b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.frisbeebot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java new file mode 100644 index 0000000000..83a289d4b4 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.frisbeebot; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java new file mode 100644 index 0000000000..2b7ed9f4ba --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java @@ -0,0 +1,119 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.frisbeebot; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem; +import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoShootTimeSeconds; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants.kAutoTimeoutSeconds; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + + // A simple autonomous routine that shoots the loaded frisbees + private final Command m_autoCommand = + // Start the command by spinning up the shooter... + new InstantCommand(m_shooter::enable, m_shooter).andThen( + // Wait until the shooter is at speed before feeding the frisbees + new WaitUntilCommand(m_shooter::atSetpoint), + // Start running the feeder + new InstantCommand(m_shooter::runFeeder, m_shooter), + // Shoot for the specified time + new WaitCommand(kAutoShootTimeSeconds)) + // Add a timeout (will end the command if, for instance, the shooter never gets up to + // speed) + .withTimeout(kAutoTimeoutSeconds) + // When the command ends, turn off the shooter and the feeder + .whenFinished(() -> { + m_shooter.disable(); + m_shooter.stopFeeder(); + }); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive + .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Spin up the shooter when the 'A' button is pressed + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(new InstantCommand(m_shooter::enable, m_shooter)); + + // Turn off the shooter when the 'B' button is pressed + new JoystickButton(m_driverController, Button.kB.value) + .whenPressed(new InstantCommand(m_shooter::disable, m_shooter)); + + // Run the feeder when the 'X' button is held, but only if the shooter is at speed + new JoystickButton(m_driverController, Button.kX.value).whenPressed(new ConditionalCommand( + // Run the feeder + new InstantCommand(m_shooter::runFeeder, m_shooter), + // Do nothing + new InstantCommand(), + // Determine which of the above to do based on whether the shooter has reached the + // desired speed + m_shooter::atSetpoint)).whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter)); + + // Drive at half speed when the bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(.5)) + .whenReleased(() -> m_robotDrive.setMaxOutput(1)); + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return m_autoCommand; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..c9d8729e0d --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants.kRightMotor2Port; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), + new PWMVictorSPX(kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), + new PWMVictorSPX(kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the two encoders. + * + * @return the average of the two encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000000..4c0fe5755e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; + +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kD; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderPorts; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kEncoderReversed; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederMotorPort; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kFeederSpeed; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kI; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kP; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kSFractional; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterMotorPort; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterTargetRPS; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kShooterToleranceRPS; +import static edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants.kVFractional; + +public class ShooterSubsystem extends PIDSubsystem { + private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(kShooterMotorPort); + private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(kFeederMotorPort); + private final Encoder m_shooterEncoder = + new Encoder(kEncoderPorts[0], kEncoderPorts[1], kEncoderReversed); + + /** + * The shooter subsystem for the robot. + */ + public ShooterSubsystem() { + super(new PIDController(kP, kI, kD)); + getController().setAbsoluteTolerance(kShooterToleranceRPS); + m_shooterEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + @Override + public void useOutput(double output) { + // Use a feedforward of the form kS + kV * velocity + m_shooterMotor.set(output + kSFractional + kVFractional * kShooterTargetRPS); + } + + @Override + public double getSetpoint() { + return kShooterTargetRPS; + } + + @Override + public double getMeasurement() { + return m_shooterEncoder.getRate(); + } + + public boolean atSetpoint() { + return m_controller.atSetpoint(); + } + + public void runFeeder() { + m_feederMotor.set(kFeederSpeed); + } + + public void stopFeeder() { + m_feederMotor.set(0); + } + + @Override + public void disable() { + super.disable(); + // Turn off motor when we disable, since useOutput(0) doesn't stop the motor due to our + // feedforward + m_shooterMotor.set(0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Main.java new file mode 100644 index 0000000000..5bafd2f484 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Main.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew; + +import edu.wpi.first.wpilibj.RobotBase; + +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Robot.java new file mode 100644 index 0000000000..f0f960f6de --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/Robot.java @@ -0,0 +1,113 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/RobotContainer.java new file mode 100644 index 0000000000..f8095df92c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/RobotContainer.java @@ -0,0 +1,127 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.Hand; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Autonomous; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.CloseClaw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.OpenClaw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Pickup; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.Place; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.PrepareToPickup; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.SetElevatorSetpoint; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.SetWristSetpoint; +import edu.wpi.first.wpilibj.examples.gearsbotnew.commands.TankDrive; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + private final DriveTrain m_drivetrain = new DriveTrain(); + private final Elevator m_elevator = new Elevator(); + private final Wrist m_wrist = new Wrist(); + private final Claw m_claw = new Claw(); + + private final Joystick m_joystick = new Joystick(0); + + private final CommandBase m_autonomousCommand = + new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Put Some buttons on the SmartDashboard + SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator)); + SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2, m_elevator)); + SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3, m_elevator)); + + SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist)); + SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist)); + + SmartDashboard.putData("Open Claw", new OpenClaw(m_claw)); + SmartDashboard.putData("Close Claw", new CloseClaw(m_claw)); + + SmartDashboard + .putData("Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator)); + + // Assign default commands + m_drivetrain.setDefaultCommand(new TankDrive(m_drivetrain, () -> m_joystick.getY(Hand.kLeft), + () -> m_joystick.getY(Hand.kRight))); + + // Show what command your subsystem is running on the SmartDashboard + SmartDashboard.putData(m_drivetrain); + SmartDashboard.putData(m_elevator); + SmartDashboard.putData(m_wrist); + SmartDashboard.putData(m_claw); + + // Call log method on all subsystems + m_wrist.log(); + m_elevator.log(); + m_drivetrain.log(); + m_claw.log(); + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Create some buttons + final JoystickButton dpadUp = new JoystickButton(m_joystick, 5); + final JoystickButton dpadRight = new JoystickButton(m_joystick, 6); + final JoystickButton dpadDown = new JoystickButton(m_joystick, 7); + final JoystickButton dpadLeft = new JoystickButton(m_joystick, 8); + final JoystickButton l2 = new JoystickButton(m_joystick, 9); + final JoystickButton r2 = new JoystickButton(m_joystick, 10); + final JoystickButton l1 = new JoystickButton(m_joystick, 11); + final JoystickButton r1 = new JoystickButton(m_joystick, 12); + + // Connect the buttons to commands + dpadUp.whenPressed(new SetElevatorSetpoint(0.2, m_elevator)); + dpadDown.whenPressed(new SetElevatorSetpoint(-0.2, m_elevator)); + dpadRight.whenPressed(new CloseClaw(m_claw)); + dpadLeft.whenPressed(new OpenClaw(m_claw)); + + r1.whenPressed(new PrepareToPickup(m_claw, m_wrist, m_elevator)); + r2.whenPressed(new Pickup(m_claw, m_wrist, m_elevator)); + l1.whenPressed(new Place(m_claw, m_wrist, m_elevator)); + l2.whenPressed(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator)); + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return m_autonomousCommand; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Autonomous.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Autonomous.java new file mode 100644 index 0000000000..152bea5135 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Autonomous.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist; + +/** + * The main autonomous command to pickup and deliver the soda to the box. + */ +public class Autonomous extends SequentialCommandGroup { + /** + * Create a new autonomous command. + */ + public Autonomous(DriveTrain drive, Claw claw, Wrist wrist, Elevator elevator) { + addCommands( + new PrepareToPickup(claw, wrist, elevator), + new Pickup(claw, wrist, elevator), + new SetDistanceToBox(0.10, drive), + // new DriveStraight(4), // Use encoders if ultrasonic is broken + new Place(claw, wrist, elevator), + new SetDistanceToBox(0.60, drive), + // new DriveStraight(-2), // Use Encoders if ultrasonic is broken + parallel( + new SetWristSetpoint(-45, wrist), + new CloseClaw(claw) + ) + ); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/CloseClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/CloseClaw.java new file mode 100644 index 0000000000..be21881aa8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/CloseClaw.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.Robot; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; + +/** + * Closes the claw for one second. Real robots should use sensors, stalling motors is BAD! + */ +public class CloseClaw extends CommandBase { + private final Claw m_claw; + + public CloseClaw(Claw claw) { + m_claw = claw; + addRequirements(m_claw); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + m_claw.close(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return m_claw.isGrabbing(); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + // NOTE: Doesn't stop in simulation due to lower friction causing the + // can to fall out + // + there is no need to worry about stalling the motor or crushing the + // can. + if (!Robot.isSimulation()) { + m_claw.stop(); + } + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/DriveStraight.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/DriveStraight.java new file mode 100644 index 0000000000..58389252c7 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/DriveStraight.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain; + +/** + * Drive the given distance straight (negative values go backwards). Uses a + * local PID controller to run a simple PID loop that is only enabled while this + * command is running. The input is the averaged values of the left and right + * encoders. + */ +public class DriveStraight extends PIDCommand { + private final DriveTrain m_drivetrain; + + /** + * Create a new DriveStraight command. + * @param distance The distance to drive + */ + public DriveStraight(double distance, DriveTrain drivetrain) { + super(new PIDController(4, 0, 0), + drivetrain::getDistance, + distance, + d -> drivetrain.drive(d, d)); + + m_drivetrain = drivetrain; + addRequirements(m_drivetrain); + + getController().setAbsoluteTolerance(0.01); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + // Get everything in a safe starting state. + m_drivetrain.reset(); + super.initialize(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return getController().atSetpoint(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/OpenClaw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/OpenClaw.java new file mode 100644 index 0000000000..d25b78ca14 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/OpenClaw.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj2.command.WaitCommand; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; + +/** + * Opens the claw for one second. Real robots should use sensors, stalling motors is BAD! + */ +public class OpenClaw extends WaitCommand { + private final Claw m_claw; + + /** + * Creates a new OpenClaw command. + * + * @param claw The claw to use + */ + public OpenClaw(Claw claw) { + super(1); + m_claw = claw; + addRequirements(m_claw); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + m_claw.open(); + super.initialize(); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + m_claw.stop(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Pickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Pickup.java new file mode 100644 index 0000000000..a13d42f907 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Pickup.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist; + +/** + * Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around. + */ +public class Pickup extends SequentialCommandGroup { + /** + * Create a new pickup command. + * + * @param claw The claw subsystem to use + * @param wrist The wrist subsystem to use + * @param elevator The elevator subsystem to use + */ + public Pickup(Claw claw, Wrist wrist, Elevator elevator) { + addCommands( + new CloseClaw(claw), + parallel( + new SetWristSetpoint(-45, wrist), + new SetElevatorSetpoint(0.25, elevator))); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Place.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Place.java new file mode 100644 index 0000000000..49168287c9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/Place.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist; + +/** + * Place a held soda can onto the platform. + */ +public class Place extends SequentialCommandGroup { + /** + * Create a new place command. + * + * @param claw The claw subsystem to use + * @param wrist The wrist subsystem to use + * @param elevator The elevator subsystem to use + */ + public Place(Claw claw, Wrist wrist, Elevator elevator) { + addCommands( + new SetElevatorSetpoint(0.25, elevator), + new SetWristSetpoint(0, wrist), + new OpenClaw(claw)); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/PrepareToPickup.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/PrepareToPickup.java new file mode 100644 index 0000000000..27e3b11421 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/PrepareToPickup.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Claw; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator; +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist; + +/** + * Make sure the robot is in a state to pickup soda cans. + */ +public class PrepareToPickup extends SequentialCommandGroup { + /** + * Create a new prepare to pickup command. + * + * @param claw The claw subsystem to use + * @param wrist The wrist subsystem to use + * @param elevator The elevator subsystem to use + */ + public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) { + addCommands( + new OpenClaw(claw), + parallel( + new SetWristSetpoint(0, wrist), + new SetElevatorSetpoint(0, elevator))); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetDistanceToBox.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetDistanceToBox.java new file mode 100644 index 0000000000..ed69814eab --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetDistanceToBox.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain; + + +/** + * Drive until the robot is the given distance away from the box. Uses a local + * PID controller to run a simple PID loop that is only enabled while this + * command is running. The input is the averaged values of the left and right + * encoders. + */ +public class SetDistanceToBox extends PIDCommand { + private final DriveTrain m_drivetrain; + + /** + * Create a new set distance to box command. + * + * @param distance The distance away from the box to drive to + */ + public SetDistanceToBox(double distance, DriveTrain drivetrain) { + super(new PIDController(-2, 0, 0), + drivetrain::getDistanceToObstacle, distance, + d -> drivetrain.drive(d, d)); + + m_drivetrain = drivetrain; + addRequirements(m_drivetrain); + + getController().setAbsoluteTolerance(0.01); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + // Get everything in a safe starting state. + m_drivetrain.reset(); + super.initialize(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return getController().atSetpoint(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetElevatorSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetElevatorSetpoint.java new file mode 100644 index 0000000000..0f9e86489b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetElevatorSetpoint.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Elevator; + + +/** + * Move the elevator to a given location. This command finishes when it is within the tolerance, but + * leaves the PID loop running to maintain the position. Other commands using the elevator should + * make sure they disable PID! + */ +public class SetElevatorSetpoint extends CommandBase { + private final Elevator m_elevator; + private final double m_setpoint; + + /** + * Create a new SetElevatorSetpoint command. + * + * @param setpoint The setpoint to set the elevator to + * @param elevator The elevator to use + */ + public SetElevatorSetpoint(double setpoint, Elevator elevator) { + m_elevator = elevator; + m_setpoint = setpoint; + addRequirements(m_elevator); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + m_elevator.setSetpoint(m_setpoint); + m_elevator.enable(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return m_elevator.getController().atSetpoint(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetWristSetpoint.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetWristSetpoint.java new file mode 100644 index 0000000000..2459dffbae --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/SetWristSetpoint.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.Wrist; + + +/** + * Move the wrist to a given angle. This command finishes when it is within the tolerance, but + * leaves the PID loop running to maintain the position. Other commands using the wrist should make + * sure they disable PID! + */ +public class SetWristSetpoint extends CommandBase { + private final Wrist m_wrist; + private final double m_setpoint; + + /** + * Create a new SetWristSetpoint command. + * + * @param setpoint The setpoint to set the wrist to + * @param wrist The wrist to use + */ + public SetWristSetpoint(double setpoint, Wrist wrist) { + m_wrist = wrist; + m_setpoint = setpoint; + addRequirements(m_wrist); + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + m_wrist.enable(); + m_wrist.setSetpoint(m_setpoint); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return m_wrist.getController().atSetpoint(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/TankDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/TankDrive.java new file mode 100644 index 0000000000..8af7b81f2f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/commands/TankDrive.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.commands; + + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems.DriveTrain; + +/** + * Have the robot drive tank style. + */ +public class TankDrive extends CommandBase { + private final DriveTrain m_drivetrain; + private final DoubleSupplier m_left; + private final DoubleSupplier m_right; + + /** + * Creates a new TankDrive command. + * + * @param drivetrain The drivetrain subsystem to drive + * @param left The control input for the left side of the drive + * @param right The control input for the right sight of the drive + */ + public TankDrive(DriveTrain drivetrain, DoubleSupplier left, DoubleSupplier right) { + m_drivetrain = drivetrain; + m_left = left; + m_right = right; + addRequirements(m_drivetrain); + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + m_drivetrain.drive(m_left.getAsDouble(), m_right.getAsDouble()); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return false; // Runs until interrupted + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + m_drivetrain.drive(0, 0); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Claw.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Claw.java new file mode 100644 index 0000000000..56de53e764 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Claw.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Victor; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * The claw subsystem is a simple system with a motor for opening and closing. If using stronger + * motors, you should probably use a sensor so that the motors don't stall. + */ +public class Claw extends SubsystemBase { + private final Victor m_motor = new Victor(7); + private final DigitalInput m_contact = new DigitalInput(5); + + /** + * Create a new claw subsystem. + */ + public Claw() { + // Let's name everything on the LiveWindow + addChild("Motor", m_motor); + addChild("Limit Switch", m_contact); + } + + public void log() { + SmartDashboard.putData("Claw switch", m_contact); + } + + /** + * Set the claw motor to move in the open direction. + */ + public void open() { + m_motor.set(-1); + } + + /** + * Set the claw motor to move in the close direction. + */ + public void close() { + m_motor.set(1); + } + + /** + * Stops the claw motor from moving. + */ + public void stop() { + m_motor.set(0); + } + + /** + * Return true when the robot is grabbing an object hard enough to trigger the limit switch. + */ + public boolean isGrabbing() { + return m_contact.get(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/DriveTrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/DriveTrain.java new file mode 100644 index 0000000000..dd9a8f3237 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/DriveTrain.java @@ -0,0 +1,124 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems; + +import edu.wpi.first.wpilibj.AnalogGyro; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +public class DriveTrain extends SubsystemBase { + /** + * The DriveTrain subsystem incorporates the sensors and actuators attached to the robots chassis. + * These include four drive motors, a left and right encoder and a gyro. + */ + private final SpeedController m_leftMotor = + new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1)); + private final SpeedController m_rightMotor = + new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3)); + + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor); + + private final Encoder m_leftEncoder = new Encoder(1, 2); + private final Encoder m_rightEncoder = new Encoder(3, 4); + private final AnalogInput m_rangefinder = new AnalogInput(6); + private final AnalogGyro m_gyro = new AnalogGyro(1); + + /** + * Create a new drive train subsystem. + */ + public DriveTrain() { + super(); + + // Encoders may measure differently in the real world and in + // simulation. In this example the robot moves 0.042 barleycorns + // per tick in the real world, but the simulated encoders + // simulate 360 tick encoders. This if statement allows for the + // real robot to handle this difference in devices. + if (Robot.isReal()) { + m_leftEncoder.setDistancePerPulse(0.042); + m_rightEncoder.setDistancePerPulse(0.042); + } else { + // Circumference in ft = 4in/12(in/ft)*PI + m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); + m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0); + } + + // Let's name the sensors on the LiveWindow + addChild("Drive", m_drive); + addChild("Left Encoder", m_leftEncoder); + addChild("Right Encoder", m_rightEncoder); + addChild("Rangefinder", m_rangefinder); + addChild("Gyro", m_gyro); + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + public void log() { + SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance()); + SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance()); + SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate()); + SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate()); + SmartDashboard.putNumber("Gyro", m_gyro.getAngle()); + } + + /** + * Tank style driving for the DriveTrain. + * + * @param left Speed in range [-1,1] + * @param right Speed in range [-1,1] + */ + public void drive(double left, double right) { + m_drive.tankDrive(left, right); + } + + /** + * Get the robot's heading. + * + * @return The robots heading in degrees. + */ + public double getHeading() { + return m_gyro.getAngle(); + } + + /** + * Reset the robots sensors to the zero states. + */ + public void reset() { + m_gyro.reset(); + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Get the average distance of the encoders since the last reset. + * + * @return The distance driven (average of left and right encoders). + */ + public double getDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2; + } + + /** + * Get the distance to the obstacle. + * + * @return The distance to the obstacle detected by the rangefinder. + */ + public double getDistanceToObstacle() { + // Really meters in simulation since it's a rangefinder... + return m_rangefinder.getAverageVoltage(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Elevator.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Elevator.java new file mode 100644 index 0000000000..b22c3ba466 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Elevator.java @@ -0,0 +1,98 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.Victor; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; + +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID + * values for simulation are different than in the real world do to minor differences. + */ +public class Elevator extends PIDSubsystem { + private final Victor m_motor; + private final AnalogPotentiometer m_pot; + private double m_setpoint; + + private static final double kP_real = 4; + private static final double kI_real = 0.07; + private static final double kP_simulation = 18; + private static final double kI_simulation = 0.2; + + /** + * Create a new elevator subsystem. + */ + public Elevator() { + super(new PIDController(kP_real, kI_real, 0)); + if (Robot.isSimulation()) { // Check for simulation and update PID values + getController().setPID(kP_simulation, kI_simulation, 0); + } + getController().setAbsoluteTolerance(0.005); + + m_motor = new Victor(5); + + // Conversion value of potentiometer varies between the real world and + // simulation + if (Robot.isReal()) { + m_pot = new AnalogPotentiometer(2, -2.0 / 5); + } else { + m_pot = new AnalogPotentiometer(2); // Defaults to meters + } + + // Let's name everything on the LiveWindow + addChild("Motor", m_motor); + addChild("Pot", m_pot); + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + public void log() { + SmartDashboard.putData("Elevator Pot", m_pot); + } + + /** + * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem. + */ + @Override + public double getMeasurement() { + return m_pot.get(); + } + + /** + * Use the motor as the PID output. This method is automatically called by the subsystem. + */ + @Override + public void useOutput(double output) { + m_motor.set(output); + } + + /** + * Returns the setpoint used by the PIDController. + * + * @return The setpoint for the PIDController. + */ + @Override + public double getSetpoint() { + return m_setpoint; + } + + /** + * Sets the setpoint used by the PIDController. + * + * @param setpoint The setpoint for the PIDController. + */ + public void setSetpoint(double setpoint) { + m_setpoint = setpoint; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Wrist.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Wrist.java new file mode 100644 index 0000000000..cd43beaf01 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbotnew/subsystems/Wrist.java @@ -0,0 +1,95 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gearsbotnew.subsystems; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.Victor; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDSubsystem; + +import edu.wpi.first.wpilibj.examples.gearsbot.Robot; + +/** + * The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint. + */ +public class Wrist extends PIDSubsystem { + private final Victor m_motor; + private final AnalogPotentiometer m_pot; + private double m_setpoint; + + private static final double kP_real = 1; + private static final double kP_simulation = 0.05; + + /** + * Create a new wrist subsystem. + */ + public Wrist() { + super(new PIDController(kP_real, 0, 0)); + if (Robot.isSimulation()) { // Check for simulation and update PID values + getController().setPID(kP_simulation, 0, 0); + } + getController().setAbsoluteTolerance(2.5); + + m_motor = new Victor(6); + + // Conversion value of potentiometer varies between the real world and + // simulation + if (Robot.isReal()) { + m_pot = new AnalogPotentiometer(3, -270.0 / 5); + } else { + m_pot = new AnalogPotentiometer(3); // Defaults to degrees + } + + // Let's name everything on the LiveWindow + addChild("Motor", m_motor); + addChild("Pot", m_pot); + } + + /** + * The log method puts interesting information to the SmartDashboard. + */ + public void log() { + SmartDashboard.putData("Wrist Angle", m_pot); + } + + /** + * Use the potentiometer as the PID sensor. This method is automatically called by the subsystem. + */ + @Override + public double getMeasurement() { + return m_pot.get(); + } + + /** + * Use the motor as the PID output. This method is automatically called by the subsystem. + */ + @Override + public void useOutput(double output) { + m_motor.set(output); + } + + /** + * Returns the setpoint used by the PIDController. + * + * @return The setpoint for the PIDController. + */ + @Override + public double getSetpoint() { + return m_setpoint; + } + + /** + * Sets the setpoint used by the PIDController. + * + * @param setpoint The setpoint for the PIDController. + */ + public void setSetpoint(double setpoint) { + m_setpoint = setpoint; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java new file mode 100644 index 0000000000..50afb7b65c --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterInches = 6; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR; + + public static final boolean kGyroReversed = false; + + public static final double kStabilizationP = 1; + public static final double kStabilizationI = .5; + public static final double kStabilizationD = 0; + + public static final double kTurnP = 1; + public static final double kTurnI = 0; + public static final double kTurnD = 0; + + public static final double kTurnToleranceDeg = 5; + public static final double kTurnRateToleranceDegPerS = 10; // degrees per second + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java new file mode 100644 index 0000000000..f18e95c3cf --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java new file mode 100644 index 0000000000..40c6db90b8 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java @@ -0,0 +1,121 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java new file mode 100644 index 0000000000..a1a99e46bc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java @@ -0,0 +1,100 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PIDCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle; +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationD; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationI; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kStabilizationP; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive + .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive)); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Drive at half speed when the right bumper is held + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(.5)) + .whenReleased(() -> m_robotDrive.setMaxOutput(1)); + + // Stabilize robot to drive straight with gyro when left bumper is held + new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld( + new PIDCommand( + new PIDController(kStabilizationP, kStabilizationI, kStabilizationD), + // Close the loop on the turn rate + m_robotDrive::getTurnRate, + // Setpoint is 0 + 0, + // Pipe the output to the turning controls + output -> m_robotDrive + .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output), + // Require the robot drive + m_robotDrive)); + + // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout + new JoystickButton(m_driverController, Button.kX.value) + .whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5)); + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // no auto + return new InstantCommand(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java new file mode 100644 index 0000000000..996748aa11 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands; + +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; + +import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem; + +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnD; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnI; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnP; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnRateToleranceDegPerS; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kTurnToleranceDeg; + +/** + * A command that will turn the robot to the specified angle. + */ +public class TurnToAngle extends PIDCommand { + /** + * Turns to robot to the specified angle. + * + * @param targetAngleDegrees The angle to turn to + * @param drive The drive subsystem to use + */ + public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) { + super(new PIDController(kTurnP, kTurnI, kTurnD), + // Close loop on heading + drive::getHeading, + // Set reference to target + targetAngleDegrees, + // Pipe output to turn robot + output -> drive.arcadeDrive(0, output), + // Require the drive + drive); + + // Set the controller to be continuous (because it is an angle controller) + getController().enableContinuousInput(-180, 180); + // Set the controller tolerance - the delta tolerance ensures the robot is stationary at the + // setpoint before it is considered as having reached the reference + getController().setAbsoluteTolerance(kTurnToleranceDeg, kTurnRateToleranceDegPerS); + } + + @Override + public boolean isFinished() { + // End when the controller is at the reference. + return getController().atSetpoint(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..e91b8b3002 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java @@ -0,0 +1,141 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kGyroReversed; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants.kRightMotor2Port; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), + new PWMVictorSPX(kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), + new PWMVictorSPX(kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + + // The gyro sensor + private final Gyro m_gyro = new ADXRS450_Gyro(); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the two encoders. + * + * @return the average of the two encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } + + /** + * Zeroes the heading of the robot. + */ + public void zeroHeading() { + m_gyro.reset(); + } + + /** + * Returns the heading of the robot. + * + * @return the robot's heading in degrees, from 180 to 180 + */ + public double getHeading() { + return Math.IEEEremainder(m_gyro.getAngle(), 360) * (kGyroReversed ? -1. : 1.); + } + + /** + * Returns the turn rate of the robot. + * + * @return The turn rate of the robot, in degrees per second + */ + public double getTurnRate() { + return m_gyro.getRate() * (kGyroReversed ? -1. : 1.); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java new file mode 100644 index 0000000000..27cb872589 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterInches = 6; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR; + } + + public static final class HatchConstants { + public static final int kHatchSolenoidModule = 0; + public static final int[] kHatchSolenoidPorts = new int[]{0, 1}; + } + + public static final class AutoConstants { + public static final double kAutoDriveDistanceInches = 60; + public static final double kAutoBackupDistanceInches = 20; + public static final double kAutoDriveSpeed = .5; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java new file mode 100644 index 0000000000..3852d41f83 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java new file mode 100644 index 0000000000..c0c78ac261 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java @@ -0,0 +1,113 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java new file mode 100644 index 0000000000..07a31c3b63 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem(); + + // The autonomous routines + + // A simple auto routine that drives forward a specified distance, and then stops. + private final Command m_simpleAuto = + new StartEndCommand( + // Start driving forward at the start of the command + () -> m_robotDrive.arcadeDrive(kAutoDriveSpeed, 0), + // Stop driving at the end of the command + () -> m_robotDrive.arcadeDrive(0, 0), + // Requires the drive subsystem + m_robotDrive + ) + // Reset the encoders before starting + .beforeStarting(m_robotDrive::resetEncoders) + // End the command when the robot's driven distance exceeds the desired value + .interruptOn(() -> m_robotDrive.getAverageEncoderDistance() >= kAutoDriveDistanceInches); + + // A complex auto routine that drives forward, drops a hatch, and then drives backward. + private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem); + + // A chooser for autonomous commands + SendableChooser m_chooser = new SendableChooser<>(); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new RunCommand(() -> m_robotDrive.arcadeDrive( + m_driverController.getY(GenericHID.Hand.kLeft), + m_driverController.getX(GenericHID.Hand.kRight)), + m_robotDrive) + ); + + // Add commands to the autonomous command chooser + m_chooser.addOption("Simple Auto", m_simpleAuto); + m_chooser.addOption("Complex Auto", m_complexAuto); + + // Put the chooser on the dashboard + Shuffleboard.getTab("Autonomous").add(m_chooser); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Grab the hatch when the 'A' button is pressed. + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem)); + // Release the hatch when the 'B' button is pressed. + new JoystickButton(m_driverController, Button.kB.value) + .whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem)); + // While holding the shoulder button, drive at half speed + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenPressed(() -> m_robotDrive.setMaxOutput(.5)) + .whenReleased(() -> m_robotDrive.setMaxOutput(1)); + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return m_chooser.getSelected(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java new file mode 100644 index 0000000000..c3e58c8562 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.StartEndCommand; + +import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem; +import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem; + +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoBackupDistanceInches; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveDistanceInches; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants.kAutoDriveSpeed; + +/** + * A complex auto command that drives forward, releases a hatch, and then drives backward. + */ +public class ComplexAutoCommand extends SequentialCommandGroup { + /** + * Creates a new ComplexAutoCommand. + * + * @param driveSubsystem The drive subsystem this command will run on + * @param hatchSubsystem The hatch subsystem this command will run on + */ + public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) { + addCommands( + // Drive forward up to the front of the cargo ship + new StartEndCommand( + // Start driving forward at the start of the command + () -> driveSubsystem.arcadeDrive(kAutoDriveSpeed, 0), + // Stop driving at the end of the command + () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem + ) + // Reset the encoders before starting + .beforeStarting(driveSubsystem::resetEncoders) + // End the command when the robot's driven distance exceeds the desired value + .interruptOn( + () -> driveSubsystem.getAverageEncoderDistance() >= kAutoDriveDistanceInches), + + // Release the hatch + new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem), + + // Drive backward the specified distance + new StartEndCommand( + () -> driveSubsystem.arcadeDrive(-kAutoDriveSpeed, 0), + () -> driveSubsystem.arcadeDrive(0, 0), + driveSubsystem + ) + .beforeStarting(driveSubsystem::resetEncoders) + .interruptOn( + () -> driveSubsystem.getAverageEncoderDistance() <= -kAutoBackupDistanceInches) + ); + } + +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..233cb8502b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants.kRightMotor2Port; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), + new PWMVictorSPX(kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), + new PWMVictorSPX(kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java new file mode 100644 index 0000000000..6dce9e1cd5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidModule; +import static edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants.kHatchSolenoidPorts; + +/** + * A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. + */ +public class HatchSubsystem extends SubsystemBase { + private final DoubleSolenoid m_hatchSolenoid = + new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]); + + /** + * Grabs the hatch. + */ + public void grabHatch() { + m_hatchSolenoid.set(kForward); + } + + /** + * Releases the hatch. + */ + public void releaseHatch() { + m_hatchSolenoid.set(kReverse); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java new file mode 100644 index 0000000000..1e9e0600d2 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final class DriveConstants { + public static final int kLeftMotor1Port = 0; + public static final int kLeftMotor2Port = 1; + public static final int kRightMotor1Port = 2; + public static final int kRightMotor2Port = 3; + + public static final int[] kLeftEncoderPorts = new int[]{0, 1}; + public static final int[] kRightEncoderPorts = new int[]{2, 3}; + public static final boolean kLeftEncoderReversed = false; + public static final boolean kRightEncoderReversed = true; + + public static final int kEncoderCPR = 1024; + public static final double kWheelDiameterInches = 6; + public static final double kEncoderDistancePerPulse = + // Assumes the encoders are directly mounted on the wheel shafts + (kWheelDiameterInches * Math.PI) / (double) kEncoderCPR; + } + + public static final class HatchConstants { + public static final int kHatchSolenoidModule = 0; + public static final int[] kHatchSolenoidPorts = new int[]{0, 1}; + } + + public static final class AutoConstants { + public static final double kAutoDriveDistanceInches = 60; + public static final double kAutoBackupDistanceInches = 20; + public static final double kAutoDriveSpeed = .5; + } + + public static final class OIConstants { + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java new file mode 100644 index 0000000000..f09858cd00 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java new file mode 100644 index 0000000000..8747f35b2a --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java new file mode 100644 index 0000000000..e4a3070052 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DefaultDrive; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.DriveDistance; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.GrabHatch; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.HalveDriveSpeed; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final HatchSubsystem m_hatchSubsystem = new HatchSubsystem(); + + // The autonomous routines + + // A simple auto routine that drives forward a specified distance, and then stops. + private final Command m_simpleAuto = + new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, m_robotDrive); + + // A complex auto routine that drives forward, drops a hatch, and then drives backward. + private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem); + + // A chooser for autonomous commands + SendableChooser m_chooser = new SendableChooser<>(); + + // The driver's controller + XboxController m_driverController = new XboxController(kDriverControllerPort); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + + // Configure default commands + // Set the default drive command to split-stick arcade drive + m_robotDrive.setDefaultCommand( + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + new DefaultDrive( + m_robotDrive, + () -> m_driverController.getY(GenericHID.Hand.kLeft), + () -> m_driverController.getX(GenericHID.Hand.kRight)) + ); + + // Add commands to the autonomous command chooser + m_chooser.addOption("Simple Auto", m_simpleAuto); + m_chooser.addOption("Complex Auto", m_complexAuto); + + // Put the chooser on the dashboard + Shuffleboard.getTab("Autonomous").add(m_chooser); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Grab the hatch when the 'A' button is pressed. + new JoystickButton(m_driverController, Button.kA.value) + .whenPressed(new GrabHatch(m_hatchSubsystem)); + // Release the hatch when the 'B' button is pressed. + new JoystickButton(m_driverController, Button.kB.value) + .whenPressed(new ReleaseHatch(m_hatchSubsystem)); + // While holding the shoulder button, drive at half speed + new JoystickButton(m_driverController, Button.kBumperRight.value) + .whenHeld(new HalveDriveSpeed(m_robotDrive)); + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return m_chooser.getSelected(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java new file mode 100644 index 0000000000..9614922353 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem; +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem; + +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoBackupDistanceInches; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveDistanceInches; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants.kAutoDriveSpeed; + +/** + * A complex auto command that drives forward, releases a hatch, and then drives backward. + */ +public class ComplexAuto extends SequentialCommandGroup { + /** + * Creates a new ComplexAuto. + * + * @param drive The drive subsystem this command will run on + * @param hatch The hatch subsystem this command will run on + */ + public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) { + addCommands( + // Drive forward the specified distance + new DriveDistance(kAutoDriveDistanceInches, kAutoDriveSpeed, drive), + + // Release the hatch + new ReleaseHatch(hatch), + + // Drive backward the specified distance + new DriveDistance(kAutoBackupDistanceInches, -kAutoDriveSpeed, drive) + ); + } + +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java new file mode 100644 index 0000000000..9399c3008e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem; + +/** + * A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written + * explicitly for pedagogical purposes - actual code should inline a command this simple with {@link + * edu.wpi.first.wpilibj2.command.RunCommand}. + */ +public class DefaultDrive extends CommandBase { + private final DriveSubsystem m_drive; + private final DoubleSupplier m_forward; + private final DoubleSupplier m_rotation; + + /** + * Creates a new DefaultDrive. + * + * @param subsystem The drive subsystem this command wil run on. + * @param forward The control input for driving forwards/backwards + * @param rotation The control input for turning + */ + public DefaultDrive(DriveSubsystem subsystem, DoubleSupplier forward, DoubleSupplier rotation) { + m_drive = subsystem; + m_forward = forward; + m_rotation = rotation; + addRequirements(m_drive); + } + + @Override + public void execute() { + m_drive.arcadeDrive(m_forward.getAsDouble(), m_rotation.getAsDouble()); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java new file mode 100644 index 0000000000..d4abd71387 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem; + +public class DriveDistance extends CommandBase { + private final DriveSubsystem m_drive; + private final double m_distance; + private final double m_speed; + + /** + * Creates a new DriveDistance. + * + * @param inches The number of inches the robot will drive + * @param speed The speed at which the robot will drive + * @param drive The drive subsystem on which this command will run + */ + public DriveDistance(double inches, double speed, DriveSubsystem drive) { + m_distance = inches; + m_speed = speed; + m_drive = drive; + } + + @Override + public void initialize() { + m_drive.resetEncoders(); + m_drive.arcadeDrive(m_speed, 0); + } + + @Override + public void end(boolean interrupted) { + m_drive.arcadeDrive(0, 0); + } + + @Override + public boolean isFinished() { + return Math.abs(m_drive.getAverageEncoderDistance()) >= m_distance; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java new file mode 100644 index 0000000000..a30da4574b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem; + +/** + * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for + * pedagogical purposes. Actual code should inline a command this simple with {@link + * edu.wpi.first.wpilibj2.command.InstantCommand}. + */ +public class GrabHatch extends CommandBase { + // The subsystem the command runs on + private final HatchSubsystem m_hatchSubsystem; + + public GrabHatch(HatchSubsystem subsystem) { + m_hatchSubsystem = subsystem; + addRequirements(m_hatchSubsystem); + } + + @Override + public void initialize() { + m_hatchSubsystem.grabHatch(); + } + + @Override + public boolean isFinished() { + return true; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java new file mode 100644 index 0000000000..396d40d718 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem; + +public class HalveDriveSpeed extends CommandBase { + private final DriveSubsystem m_drive; + + public HalveDriveSpeed(DriveSubsystem drive) { + m_drive = drive; + } + + @Override + public void initialize() { + m_drive.setMaxOutput(.5); + } + + @Override + public void end(boolean interrupted) { + m_drive.setMaxOutput(1); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java new file mode 100644 index 0000000000..1e6f0a0f00 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java @@ -0,0 +1,21 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem; + +/** + * A command that releases the hatch. + */ +public class ReleaseHatch extends InstantCommand { + public ReleaseHatch(HatchSubsystem subsystem) { + super(subsystem::releaseHatch, subsystem); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java new file mode 100644 index 0000000000..6cadc1f031 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kEncoderDistancePerPulse; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderPorts; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftEncoderReversed; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor1Port; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kLeftMotor2Port; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderPorts; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightEncoderReversed; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor1Port; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants.kRightMotor2Port; + +public class DriveSubsystem extends SubsystemBase { + // The motors on the left side of the drive. + private final SpeedControllerGroup m_leftMotors = + new SpeedControllerGroup(new PWMVictorSPX(kLeftMotor1Port), + new PWMVictorSPX(kLeftMotor2Port)); + + // The motors on the right side of the drive. + private final SpeedControllerGroup m_rightMotors = + new SpeedControllerGroup(new PWMVictorSPX(kRightMotor1Port), + new PWMVictorSPX(kRightMotor2Port)); + + // The robot's drive + private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors); + + // The left-side drive encoder + private final Encoder m_leftEncoder = + new Encoder(kLeftEncoderPorts[0], kLeftEncoderPorts[1], kLeftEncoderReversed); + + // The right-side drive encoder + private final Encoder m_rightEncoder = + new Encoder(kRightEncoderPorts[0], kRightEncoderPorts[1], kRightEncoderReversed); + + /** + * Creates a new DriveSubsystem. + */ + public DriveSubsystem() { + // Sets the distance per pulse for the encoders + m_leftEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + m_rightEncoder.setDistancePerPulse(kEncoderDistancePerPulse); + } + + /** + * Drives the robot using arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public void arcadeDrive(double fwd, double rot) { + m_drive.arcadeDrive(fwd, rot); + } + + /** + * Resets the drive encoders to currently read a position of 0. + */ + public void resetEncoders() { + m_leftEncoder.reset(); + m_rightEncoder.reset(); + } + + /** + * Gets the average distance of the TWO encoders. + * + * @return the average of the TWO encoder readings + */ + public double getAverageEncoderDistance() { + return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2.; + } + + /** + * Gets the left drive encoder. + * + * @return the left drive encoder + */ + public Encoder getLeftEncoder() { + return m_leftEncoder; + } + + /** + * Gets the right drive encoder. + * + * @return the right drive encoder + */ + public Encoder getRightEncoder() { + return m_rightEncoder; + } + + /** + * Sets the max output of the drive. Useful for scaling the drive to drive more slowly. + * + * @param maxOutput the maximum output to which the drive will be constrained + */ + public void setMaxOutput(double maxOutput) { + m_drive.setMaxOutput(maxOutput); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java new file mode 100644 index 0000000000..e93fea46f3 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward; +import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidModule; +import static edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants.kHatchSolenoidPorts; + +/** + * A hatch mechanism actuated by a single {@link DoubleSolenoid}. + */ +public class HatchSubsystem extends SubsystemBase { + private final DoubleSolenoid m_hatchSolenoid = + new DoubleSolenoid(kHatchSolenoidModule, kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]); + + /** + * Grabs the hatch. + */ + public void grabHatch() { + m_hatchSolenoid.set(kForward); + } + + /** + * Releases the hatch. + */ + public void releaseHatch() { + m_hatchSolenoid.set(kReverse); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java index e795593f0e..4fc80553f5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -11,7 +11,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Trigger; /** - * A custom button that is triggered when two buttons on a Joystick are + * A custom button that is triggered when TWO buttons on a Joystick are * simultaneously pressed. */ public class DoubleButton extends Trigger { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java new file mode 100644 index 0000000000..899d074a99 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.schedulereventlogging; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + /** + * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain + * access to the constants contained within without having to preface the names with the class, + * greatly reducing the amount of text required. + */ + public static final class OIConstants { + // Example: the port of the driver's controller + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java new file mode 100644 index 0000000000..fda3a447d4 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.schedulereventlogging; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java new file mode 100644 index 0000000000..bfdf0d7fbc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.schedulereventlogging; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java new file mode 100644 index 0000000000..b210ddf745 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java @@ -0,0 +1,83 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.schedulereventlogging; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.EventImportance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +import static edu.wpi.first.wpilibj.XboxController.Button; +import static edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants.kDriverControllerPort; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The driver's controller + private final XboxController m_driverController = new XboxController(kDriverControllerPort); + + // A few commands that do nothing, but will demonstrate the scheduler functionality + private final CommandBase m_instantCommand1 = new InstantCommand(); + private final CommandBase m_instantCommand2 = new InstantCommand(); + private final CommandBase m_waitCommand = new WaitCommand(5); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Set names of commands + m_instantCommand1.setName("Instant Command 1"); + m_instantCommand2.setName("Instant Command 2"); + m_waitCommand.setName("Wait 5 Seconds Command"); + + // Set the scheduler to log Shuffleboard events for command initialize, interrupt, finish + CommandScheduler.getInstance().onCommandInitialize(command -> Shuffleboard.addEventMarker( + "Command initialized", command.getName(), EventImportance.kNormal)); + CommandScheduler.getInstance().onCommandInterrupt(command -> Shuffleboard.addEventMarker( + "Command interrupted", command.getName(), EventImportance.kNormal)); + CommandScheduler.getInstance().onCommandFinish(command -> Shuffleboard.addEventMarker( + "Command finished", command.getName(), EventImportance.kNormal)); + + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + // Run instant command 1 when the 'A' button is pressed + new JoystickButton(m_driverController, Button.kA.value).whenPressed(m_instantCommand1); + // Run instant command 2 when the 'X' button is pressed + new JoystickButton(m_driverController, Button.kX.value).whenPressed(m_instantCommand2); + // Run instant command 3 when the 'Y' button is held; release early to interrupt + new JoystickButton(m_driverController, Button.kY.value).whenHeld(m_waitCommand); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return new InstantCommand(); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java new file mode 100644 index 0000000000..2b73deb256 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.selectcommand; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + /** + * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain + * access to the constants contained within without having to preface the names with the class, + * greatly reducing the amount of text required. + */ + public static final class OIConstants { + // Example: the port of the driver's controller + public static final int kDriverControllerPort = 1; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java new file mode 100644 index 0000000000..a1104ac634 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.selectcommand; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java new file mode 100644 index 0000000000..8d001739a9 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java @@ -0,0 +1,120 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.selectcommand; + +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java new file mode 100644 index 0000000000..37142e883f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.selectcommand; + +import java.util.Map; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; + +import static java.util.Map.entry; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The enum used as keys for selecting the command to run. + private enum CommandSelector { + ONE, TWO, THREE + } + + // An example selector method for the selectcommand. Returns the selector that will select + // which command to run. Can base this choice on logical conditions evaluated at runtime. + private CommandSelector select() { + return CommandSelector.ONE; + } + + // An example selectcommand. Will select from the three commands based on the value returned + // by the selector method at runtime. Note that selectcommand works on Object(), so the + // selector does not have to be an enum; it could be any desired type (string, integer, + // boolean, double...) + private final Command m_exampleSelectCommand = + new SelectCommand( + // Maps selector values to commands + Map.ofEntries( + entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")), + entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")), + entry(CommandSelector.THREE, new PrintCommand("Command three was selected!")) + ), + this::select + ); + + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return m_exampleSelectCommand; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java index 7f59e14c9b..adc713dda2 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -30,7 +30,7 @@ public class Robot extends TimedRobot { @Override public void robotInit() { // Add a 'max speed' widget to a tab named 'Configuration', using a number slider - // The widget will be placed in the second column and row and will be two columns wide + // The widget will be placed in the second column and row and will be TWO columns wide m_maxSpeed = Shuffleboard.getTab("Configuration") .add("Max Speed", 1) .withWidget("Number Slider") diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java new file mode 100644 index 0000000000..01840960d7 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be + * declared globally (i.e. public static). Do not put anything functional in this class. + * + *

It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java index 84ddea4677..b529f34107 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -10,9 +10,9 @@ package edu.wpi.first.wpilibj.templates.commandbased; import edu.wpi.first.wpilibj.RobotBase; /** - * Do NOT add any static variables to this class, or any initialization at all. - * Unless you know what you are doing, do not modify this file except to - * change the parameter class to the startRobot call. + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. */ public final class Main { private Main() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java deleted file mode 100644 index 990eb2a391..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java +++ /dev/null @@ -1,42 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.templates.commandbased; - -/** - * This class is the glue that binds the controls on the physical operator - * interface to the commands and command groups that allow control of the robot. - */ -public class OI { - //// CREATING BUTTONS - // One type of button is a joystick button which is any button on a - //// joystick. - // You create one by telling it which joystick it's on and which button - // number it is. - // Joystick stick = new Joystick(port); - // Button button = new JoystickButton(stick, buttonNumber); - - // There are a few additional built in buttons you can use. Additionally, - // by subclassing Button you can create custom triggers and bind those to - // commands the same as any other Button. - - //// TRIGGERING COMMANDS WITH BUTTONS - // Once you have a button, it's trivial to bind it to a button in one of - // three ways: - - // Start the command when the button is pressed and let it run the command - // until it is finished as determined by it's isFinished method. - // button.whenPressed(new ExampleCommand()); - - // Run the command while the button is being held down and interrupt it once - // the button is released. - // button.whileHeld(new ExampleCommand()); - - // Start the command when the button is released and let it run the command - // until it is finished as determined by it's isFinished method. - // button.whenReleased(new ExampleCommand()); -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java index 7727bff90f..c0bc795c36 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -8,55 +8,49 @@ package edu.wpi.first.wpilibj.templates.commandbased; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand; -import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; /** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the TimedRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.gradle file in the + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the * project. */ public class Robot extends TimedRobot { - public static ExampleSubsystem m_subsystem = new ExampleSubsystem(); - public static OI m_oi; + private Command m_autonomousCommand; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + private RobotContainer m_robotContainer; /** - * This function is run when the robot is first started up and should be - * used for any initialization code. + * This function is run when the robot is first started up and should be used for any + * initialization code. */ @Override public void robotInit() { - m_oi = new OI(); - m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); - // chooser.addOption("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", m_chooser); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); } /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. * *

This runs after the mode specific periodic functions, but before * LiveWindow and SmartDashboard integrated updating. */ @Override public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); } /** * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. */ @Override public void disabledInit() { @@ -64,34 +58,18 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - Scheduler.getInstance().run(); } /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString code to get the auto name from the text box below the Gyro - * - *

You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons - * to the switch structure below with additional strings & commands. + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); - - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } - */ + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) if (m_autonomousCommand != null) { - m_autonomousCommand.start(); + m_autonomousCommand.schedule(); } } @@ -100,7 +78,6 @@ public class Robot extends TimedRobot { */ @Override public void autonomousPeriodic() { - Scheduler.getInstance().run(); } @Override @@ -119,7 +96,12 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - Scheduler.getInstance().run(); + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java new file mode 100644 index 0000000000..5fbf99f34b --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.templates.commandbased; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand; +import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.Command; + +/** + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot + * (including subsystems, commands, and button mappings) should be declared here. + */ +public class RobotContainer { + // The robot's subsystems and commands are defined here... + private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem(); + + private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); + + + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a + * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + } + + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An ExampleCommand will run in autonomous + return m_autoCommand; + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java deleted file mode 100644 index ef213c466d..0000000000 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java +++ /dev/null @@ -1,26 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.templates.commandbased; - -/** - * The RobotMap is a mapping from the ports sensors and actuators are wired into - * to a variable name. This provides flexibility changing wiring, makes checking - * the wiring easier and significantly reduces the number of magic numbers - * floating around. - */ -public class RobotMap { - // For example to map the left and right motors, you could define the - // following variables to use with your drivetrain subsystem. - // public static int leftMotor = 1; - // public static int rightMotor = 2; - - // If you are using multiple modules, make sure to define both the port - // number and the module. For example you with a rangefinder: - // public static int rangefinderPort = 1; - // public static int rangefinderModule = 1; -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java index 10ceb6e7ff..cc79077900 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,42 +7,23 @@ package edu.wpi.first.wpilibj.templates.commandbased.commands; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.templates.commandbased.Robot; +import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; /** - * An example command. You can replace me with your own command. + * An example command that uses an example subsystem. */ -public class ExampleCommand extends Command { - public ExampleCommand() { - // Use requires() here to declare subsystem dependencies - requires(Robot.m_subsystem); - } +public class ExampleCommand extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ExampleSubsystem m_subsystem; - // Called just before this Command runs the first time - @Override - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ExampleCommand(ExampleSubsystem subsystem) { + m_subsystem = subsystem; + addRequirements(subsystem); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java index a03b73f0ca..b260c7778a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,18 +7,21 @@ package edu.wpi.first.wpilibj.templates.commandbased.subsystems; -import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; -/** - * An example subsystem. You can replace me with your own Subsystem. - */ -public class ExampleSubsystem extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. +public class ExampleSubsystem extends SubsystemBase { + /** + * Creates a new ExampleSubsystem. + */ + public ExampleSubsystem() { + } + + /** + * Will be called periodically whenever the CommandScheduler runs. + */ @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); + public void periodic() { + } }