== Command Based Programming ==

As our robot programs become more complex, a framework is needed to help keep related functionality together (''cohesion'') while preventing unrelated functions from becoming entangled (''coupling'').  Improving cohesion and reducing coupling are key concepts in software engineering.  WPILib provides a framework called Command Based Programming that helps with this.  You can read about it in [https://wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming ScreenStepsLive].

In command-based programming, we define 
   * '''subsystems''' representing ''physical parts'' of our robot: intakes, shooters, turrets, drivetrains, etc.
   * '''commands''' representing ''activities'' that can be done by one or more subsystem (e.g. throwing a frisbee)
   * '''command groups''' represent ''behaviors'': a set of activities that must be performed in a particular sequence (such as pointing a turret at a target, adjusting the elevation angle, and then throwing a frisbee into it).

A given activity may require more than one subsystem (e.g. throwing a frisbee might require both the turret and the shooter).  More than one activity can be running at the same time (e.g. the robot might be driving while throwing frisbees).  The Command-Based framework helps organize all of this activity so that multiple things can be happening at the same time without interfering with each other (e.g. preventing two commands from trying to control of the same subsystem at the same time).

=== Creating a Command-Based Program ===
Create a new project in VSCode (WPILib Icon->Create a new project->Template->Java->Command Robot).  Notice that in the project src folder, addition to the Main.java and Robot.java we're used to seeing, the project also has: [[Image(CommandBasedFiles.jpg,align=right,margin=10)]]
   * !RobotContainer.java - where the subsystems, commands, and button bindings are defined
   * Constants.java - where global constants are defined (e.g. motor controller addresses) - may be called !RobotMap
   * commands folder - where your Commands will be stored
   * subsystems folder - where your Subsystems will be stored
Robot.java still extends !TimedRobot (the framework we've been using until now) and the methods robotInit(), robotPeriodic(), autonomousInit(), autonomousPeriodic(), teleopInit(), teleopPeriodic().  However, it now creates and instance of the !RobotContainer class that creates the robot subsystems, user interface objects(xbox controller, joysticks, etc.) and binds button presses to the activities (commands, command groups) that your robot will be performing.  For example, when the X button is pressed on your controller run a command to launch a frisbee.

=== Example Code ===
!ScreenStepsLive has a very good example of how a program might look if programmed for !TimedRobot and then the same program converted to !CommandRobot; you should review it [https://wpilib.screenstepslive.com/s/currentCS/m/cpp/l/241906-converting-a-simple-autonomous-program-to-a-command-based-autonomous-program here]

The following is an example of a very simple Command Based Robot that has a single subsystem consisting of a servo motor with two positions: Open, Closed and they are selected using the A and B buttons on a gamepad.  The servo motor should be connected to PWM port 0 and the code uses a Logitech F310 for the operator interface.  Replace the template files with:

Constants.java:
{{{
package frc.robot;

public final class Constants {
    public static int SERVO_PORT = 0;   // PWM port for servo motor
    public static int OPEN_BUTTON = 1;  // A button opens mechanism
    public static int CLOSE_BUTTON = 2; // B button closes mechanism    
}
}}}

Add !ServoSubsystem.java under subsystems:
{{{
package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
import edu.wpi.first.wpilibj.Servo;

public class ServoSubsystem extends SubsystemBase {
  private Servo servo;

  private static ServoSubsystem instance = null;

  private ServoSubsystem() {
      servo = new Servo(Constants.SERVO_PORT);
  }

  // Singleton design pattern
  public static ServoSubsystem getInstance() {
      if (instance == null) {
          instance = new ServoSubsystem();
      }
      return instance;
  }

  public void setAngle(double degrees) {
    servo.setAngle(degrees);
  }
}
}}}

Add new commands in commands folder:

!OpenCommand.java:
{{{
package frc.robot.commands;

import edu.wpi.first.wpilibj.command.Command;
import frc.robot.Robot;

public class OpenCommand extends Command {
  public OpenCommand() {
    requires(Robot.m_servo); // servo must be available
  }

  @Override
  protected boolean isFinished() {
    System.out.println("Open Pressed.");
    Robot.m_servo.setAngle(30); // set servo to open position
    return true;                // and then finish
  }
}
}}}

!CloseCommand.java:
{{{
package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.subsystems.ServoSubsystem;

public class CloseCommand extends CommandBase {

    public CloseCommand(SubsystemBase servoSubsystem) {
        addRequirements(servoSubsystem);
    }

    @Override
    public boolean isFinished() {
        System.out.println("Close Pressed.");
        ServoSubsystem.getInstance().setAngle(120); // set servo to closed position
        return true;                 // and then finish
    }
}
}}}

!RobotContainer.java:
{{{
package frc.robot;

import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.button.Button;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.subsystems.ServoSubsystem;
import frc.robot.commands.OpenCommand;
import frc.robot.commands.CloseCommand;

public class RobotContainer {
  private final ServoSubsystem m_servo = ServoSubsystem.getInstance();
  private final XboxController xbox;
  private final Button openButton, closeButton;

  public RobotContainer() {
    xbox = new XboxController(0);
    openButton = new JoystickButton(xbox, Constants.OPEN_BUTTON);
    closeButton = new JoystickButton(xbox, Constants.CLOSE_BUTTON);   

    // Configure the button bindings
    configureButtonBindings();
  }

  private void configureButtonBindings() {
    openButton.whenPressed(new OpenCommand(m_servo));
    closeButton.whenPressed(new CloseCommand(m_servo));
  }

  public Command getAutonomousCommand() {
    return null;
  }
}
}}}

Robot.java:
{{{
package frc.robot;

import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.CommandScheduler;

public class Robot extends TimedRobot {
  private RobotContainer m_robotContainer;

  @Override
  public void robotInit() {
    m_robotContainer = new RobotContainer();
  }

  @Override
  public void robotPeriodic() {
    CommandScheduler.getInstance().run();
  }
}
}}}

=== How it works ===
Command-based programming is built on top of the !TimedRobot template. In each of the template's periodic() methods, !CommandScheduler.getInstance().run() is called. This takes all currently running Commands -- which behave like threads, but can't include waits like typical threads because it's a type of threading known as "cooperative" multitasking -- and runs them, first initializing any new ones and checking whether each one has completed yet. It's useful because these Commands can be easily joined together in !CommandGroups to make autonomous modes, or can be mapped to buttons to easily allow complex teleop actions, etc.

* [https://docs.google.com/document/d/1JHHRGrhe96rJXw6lIM-X3WJKoD14_8PwqjjP45UI0tg/ Intro to Command Based Programming]