If you’re lucky enough to have written code from this guide that compiles, make sure that your robot is in a position where it will not hurt anyone if something goes wrong when you run it. I am not responsible for any damaged persons or property.

Red indicates a note to myself or unfinished section. Apologies for sharing this guide prematurely :)

• ←this triangle indicates that there is a dropdown revealing more text. Click it to reveal what is hidden, click again to hide it.

  Hello. I am hidden text.

Before we get to that we need to cover some background info...

What is a “Black Box”

“Black Box” is not a technical term. It is a way of referring to some abstract thing whose task is known, but the way that it has been designed internally is unknown.

You treat a car like a “black box” as well. When you drive a car you are never aware of what is happening on the inside from gears shifting to the engine turning to the... well I actually don’t know what else, because I’ve never built a car. Although you don’t know what is happening on the inside, you know there are certain rules that need to be followed.

• You can’t drive your car from the passenger seat or from the back seat.

• You cant move forward until you turn the key in the ignition

• To “move forward” you need to press the gas pedal

• To “stop” you need to press the brake pedal

At no point while driving the car are you thinking about the brake pads clamping onto the wheels or the operation of your fuel injector. You only interact with the car in terms of the operations “start”, “stop”, “turn”, “park”, etc. Expert car users may also know about “signaling”!

/*
 *             ┌─────────┐
 * 30─────────►│    ?    │
 *             │  Black  ├────────►53
 *             │   Box   │
 * 23─────────►│    ?    │
 *             └─────────┘
 */

When writing the instructions for this robot we will use some “black boxes” that have been created for us and we will also use some that we create ourselves. By doing this we keep our code organized and easy to understand.

What is a “Class”?

Sometimes a thing that you are writing code for gets too big for one file. It becomes so big that it is no longer reasonable for ONE person, even its creator, to know exactly what every single line of code does in the program.

This is where classes become useful. Instead of having a giant soup of code, we will divide the code up into groups of variables and functions that act as “black boxes” in our system. Once we divide the code into these sections, all that the programmer will need to know is what each of these boxes can do in general, and what its role is in the overall system.

Bottom line: a class is just a way of dividing code into groups of variables and functions. By using them, we make our code organized and easy to use.

I HIGHLY RECOMMEND that you use Codecademy to become familiar with the syntax of Java and the concept of a class before moving forward. You will only need to complete sections 1-3. I will not be covering the basics of working with classes and the syntax of Java here, because I don’t think I would be able to write anything here that is better than learning by following their exercises.

Additionally, the first 9 sections of W3schools tutorials on Java Classes do a great job going further in depth if you would like additional support. It will make the rest of this guide a lot easier, because we will be working a lot with classes.

The WPI Library

Let’s remember what our main goal is. We want to connect our inputs with some kind of real mechanical motion/action in the robot. Right now this is our understanding of the system in terms of “black boxes”. It might not seem that useful to draw out our understanding at this point, but I think this helps to frame the problem that the WPI Library solves.

/*
 *                          ┌─────────────────────────────┐
 *                          │                             │
 *                          │                             ├───►Motor Movement
 * Xbox Controller─────────►│                             │
 *                          │                             ├───►Actuator Movement
 * Button──────────────────►│                             │
 *                          │             ???             ├───►Cargo Gate Opening/Closing
 * Autonomous Routine──────►│                             │
 *                          │                             │
 * On-Screen Buttons───────►│                             │
 *                          │                             │
 *                          │                             │
 *                          └─────────────────────────────┘
 */

This is a diagram of the system we will build in this guide. This collection of classes will be used to drive the robot forward at a certain speed for a certain duration when a button is pressed. To do this we will need to create our own “DriveTrain” Subsystem class, a “DriveForward” Command class, and write code in the RobotContainer class so that both these are used properly and so our button will be connected to the DriveForward command. We will cover all of this in this guide.

/*
 *                                                                    ┌─────────────────────┐
 *                                                                    │Physical Button Input│
 *                                                                    └───────┬─────────────┘
 *                                                                            │
 *                                                                            │
 * ┌────────────────────────┬───────────────┬─────────────────────────────────┼──────────────┐
 * │                        │Robot Container│                                 │              │
 * │                        └───────────────┘                                 │              │
 * │  ┌───────────────────┐                                          ┌────────▼───────┐      │
 * │  │DriveTrain Instance│                                          │Button Variable │      │
 * │  └─────┬─────────────┘                                          └────────────────┘      │
 * │        │                                                                                │
 * │        │  ┌────────────────────┬───────────────────────────────────────────────────┐    │
 * │        │  │DriveForward Command│                                                   │    │
 * │        │  ├────────────────────┘                                                   │    │
 * │        │  │                                              ┌─────┐     ┌────────┐    │    │
 * │        │  │                                              │Speed│     │Duration│    │    │
 * │        │  │                                              └──┬──┘     └───┬────┘    │    │
 * │        │  │     ┌────────────────────────────────────────┬──▼────────────▼─┐       │    │
 * │        └──┼─────►DriveForward's Local DriveTrain Variable│                 │       │    │
 * │           │     ┌────────────────────────────────────────┘                 │       │    │
 * │           │     │                                                          │       │    │
 * │           │     │                                                          │       │    │
 * │           │     │                                  ┌───────────┬───────┐   │       │    │
 * │           │     │                                  │PWMSparkMax│       │   │       │    │
 * │           │     │                                  ├───────────┘       │   │       │    │
 * │           │     │                                  │                   │   │       │    │
 * │           │     │                                  │                   │   │       │    │
 * │           │     │                                  │                   │   │       │    │
 * │           │     │                                  └──────────┬────────┘   │       │    │
 * │           │     │                                             │            │       │    │
 * │           │     └─────────────────────────────────────────────┼────────────┘       │    │
 * │           │                                                   │                    │    │
 * │           │                                                   │                    │    │
 * │           └───────────────────────────────────────────────────┼────────────────────┘    │
 * │                                                               │                         │
 * │                                                               │                         │
 * └───────────────────────────────────────────────────────────────┼─────────────────────────┘
 *                                                                 │
 *                                                                 │
 *                                                                 │
 *                                                      ┌──────────▼─────────┐
 *                                                      │Physical Motor Speed│
 *                                                      └────────────────────┘
 */
/* needs to show that the local drivetrain variable is assigned THROUGH the use of the constructor for that 
 class and show what other methods do as well*/

The Major Classes in the WPI Library

There are a few major classes used to organize the process of programming the robot.

“Constants” Class

• So a class is just a collection of variables and functions, correct? Let’s start with one of the least complex classes in this system, the “Constants” class. This class is just a collection of frequently used, unchanging values stored in variables. Literally just a class full of strings and numbers or whatever else we might use.

• The most common way this class is used is to store where each physical part is wired on the roboRIO, and so we will do that in order to show how this class works.

• Let’s go ahead and and look at an example of the “Constants” class. (expand by clicking the triangle on the left)

  // Copyright (c) FIRST and other WPILib contributors.
  // Open Source Software; you can modify and/or share it under the terms of
  // the WPILib BSD license file in the root directory of this project.
  
  package frc.robot;
  
  /**
   * 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.
   *
   * <p>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 int LEFT_MOTOR_PORT = 0;
      public static final int RIGHT_MOTOR_PORT = 1;
  }

• Most of what is written above is taken from the default template’s “Constants.java” file. The only lines I have added are the ones that declare the variables LEFT_MOTOR_PORT and RIGHT_MOTOR_PORT. Do not worry about all of the keywords public static final for right now, we will explain some of those key words in a later section.

• What this class allows us to do is give these port numbers a common name that can be used across our entire system of classes. If we choose these names intelligently, they will also act as descriptions of themselves. LEFT_MOTOR_PORT will be used to store the port number of the motor controller that our left wheel is connected to.
  • We use all caps for these variables to show that they are “CONSTANTS”. Constants are variables whose values do not ever change while the program is running. We set their value in the code when we write it and it is never set to a different value.

• Now we have what is called a “mapping” of port numbers to recognizable names. We will revisit this class and show how it can be used when we cover “Subsystems”.

*Each type of physical part has its own class*

• Each physical part of the robot that is connected to the RoboRIO micro controller will have a class associated with it. Whether that be a motor a proximity sensor, a joystick, or some other component. We do not need to make these classes ourselves or to know how they are designed.

• Example: A motor on the robot is represented by the class “PWMSparkMax” or “PWMTalonFX” or some other class data type that corresponds with the specific type of motor controller that we are using

• In order to find a class we can use for a part, we search the documentation of the WPI Library. In these documents it will show us what the class (the black box) is capable of and how it should be used.
  • It can sometimes feel overwhelming (even for me) to sort through documents, so do do not hesitate to ask for help with this (or anything for that matter lol). You won’t need to look through these documents yourself in order to follow the rest of this walkthrough, but to implement more complex systems later on you may need to.

“Subsystem” Class

• A subsystem is a collection of parts that perform a distinct task.

• We use the “Subsystem” class to divide the parts physically connected to our RoboRIO into subsystems.

• You might be asking why we even want to do this in the first place. To answer this question, let’s use our robot as an example and discuss two ways of designing it. Our robot has two primary purposes: driving and scoring in the lower goal.
  1. Driving
  2. Release cargo into the lower goal.

• Let’s first consider one way of designing our system and an alternative. One way that we could design our code is to declare a single “system” that contains all of the parts of our robot. This would mean that the motor for opening the cargo compartment (for scoring) would be found in the same class as the motor for turning our robot’s wheels. Although it is possible to design our system in this way, and it would work if we wrote it correctly, it makes our system disorganized. The motors attached to our wheels have no direct influence on the parts of our scoring system, so having them in the same class makes reading our code and fixing it painful.

• Instead we can create two different “Subsystem” classes. One that contains all of the parts needed for driving (motors for wheels) and another that contains all of the parts needed for our cargo releasing system. By doing this we create 2 “black boxes” that are easier to understand from the outside. This will make more sense after we learn how the “Command” class works.

• This is an empty template for a “DriveTrain” subsystem. This will be a drivetrain subsystem responsible for driving our robot using the motors connected to the wheels once we walk through filling it out.

  (This is a template provided by the WPI Library. I can show you how to create this file inside our project in VSCode, but for now just assume this file exists and run with it. We will also use templates for the “Command” class and the “RobotContainer” class)

  package frc.robot.subsystems;
  
  import edu.wpi.first.wpilibj2.command.SubsystemBase;
  
  public class DriveTrain extends SubsystemBase {
    /** Creates a new DriveTrain. */
    public DriveTrain() {}
  
    @Override
    public void periodic() {
      // This method will be called once per scheduler run
    }
  }

• We don’t need to pay too much attention to the first two lines of this code right now. Basically they tell our program that we are building on top of some other code in the WPI Library and those two lines tell it where to find the code we are building on top of. Sometimes when we want to use other classes in the WPI Library we need to tell it where to import those files from. This process can often times be done automatically through VSCode, I can walk you through that if it is an issue.

• The line public class DriveTrain extends SubsystemBase from the above selection of code is telling our program “hey, I am creating a new Subsystem class”
  • I explain each word of this line here. (click the triangle on the left to reveal the hidden text)

    • public - make this class available for other classes to use

    • class - the type of object that this is the same as if we were declaring a float or an integer

    • DriveTrain - this is the name of our class. If we wanted to we could have named it ElfWithAnAbacus, but this name describes the purpose of this class better making our codse easier to understand.

    • extends SubsystemBase - “SubsystemBase” is the set of requirements to make a new Subsystem class. By saying that the class we are creating “extends” the “SubsystemBase”, we are telling the computer to let us know if there are any requirements for making a Subsystem that we did not fulfill.

• Before we move to the next line, we need to declare the variables that will be part of our “DriveTrain” subsystem, so that they can be used in each of the functions that are defined afterwards
  • Earlier we mentioned how the class called “PWMSparkMax” is the class that is used to control the motors. Since we have two motors in our driving system, we will declare two variables of this type. This is what it looks like to do this.

    private final PWMSparkMax leftMotor = new PWMSparkMax(0);
    private final PWMSparkMax rightMotor = new PWMSparkMax(1);

  • Open this dropdown to see what each part of these statements means

    • private - remember when we talked about “black boxes”? This keyword helps us maintain the concept of a black box. What this word indicates is that the variable being declared can only be directly accessed within the class that it is declared. We don’t want the system outside of this black box to know about each individual motor in the drive train. We only want the rest of the system to know that they are allowed to tell the drive train to do “abstract” things like move forward/backward and turn. In contrast, the word public explicitly “exposes” a part of the class that we want the code outside this class to use.

    • final - this key word is used in order to tell the program that this variable is not supposed to be modified at all once it has been created. If some part of our code tries to change it, then there should be an error. This might sound pointless, but it is actually useful for making sure that we are using our code like we are supposed to. Instead of spending time debugging a problem that resulted from changing something we weren’t supposed to, this will make it so that we are immediately alerted of incorrect usage.
      • This only applies to things that do not change while our program is being run. If you have a variable that is supposed to change over time this safe gaurd does not apply. In this case, we know that the port that our motor is wired into will never change, so it is useful to have this safegaurd.

    • PWMSparkMax - this is what indicates what data type this is. This is an instance of a “PWMSparkMax” class

    • leftMotor - the name that we will use to refer to the instance of “PWMSparkMax” that controls left motor variable

    • new PWMSparkMax(0); - this is used to create a new instance of the “PWMSparkMax” class. It takes the “argument” 0 to indicate the port on the board where our motor is located. In this case, it is the port where our left motor is connected on the RoboRIO.
      • This information (and more about this class’s functionality) can also be found in the WPI Library documents here
  • Earlier we set up the “Constants” class to store where each part was wired to the roboRIO. Instead of using 0 or 1 in these lines, we can use the value stored in our “Constants” class. Here’s what these lines look like using the “Constants” class

    private final PWMSparkMax leftMotor = new PWMSparkMax(Constants.LEFT_MOTOR_PORT);
    private final PWMSparkMax rightMotor = new PWMSparkMax(Constants.RIGHT_MOTOR_PORT);

    Why would we want to do this instead? Imagine that at some point while building the robot, the left motor is moved from being wired to port 0 to being wired to port 5. We will need to change the code so that it matches how our robot is wired. If we are not referencing the “Constants” class, then we will need to remember every place we might have used the port 0 in our code and change it to port 5. If we forget even one place where we might have used port 0, we will introduce a bug in our code that could confuse us for days.

    Instead, if we reference the “Constants” class each time we need the port number for the left motor, we can simply change the value in the “Constants” class once and that same value will be referenced every time we need the left motor port. In addition our code will become more “readable” to new people reading our code, because instead of just seeing 0, they will see a variable name that gives context to what that value represents.

  • Here is our “DriveTrain” class so far:

    // Copyright (c) FIRST and other WPILib contributors.
    // Open Source Software; you can modify and/or share it under the terms of
    // the WPILib BSD license file in the root directory of this project.
    
    package frc.robot.subsystems;
    
    import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
    import edu.wpi.first.wpilibj2.command.SubsystemBase;
    
    public class DriveTrain extends SubsystemBase {
      /** Creates a new DriveTrain. */
      private final PWMSparkMax leftMotor = new PWMSparkMax(Constants.LEFT_MOTOR_PORT);
      private final PWMSparkMax rightMotor = new PWMSparkMax(Constants.RIGHT_MOTOR_PORT);
    
      public DriveTrain() {
      }
    
      @Override
      public void periodic() {
        // This method will be called once per scheduler run
      }
    }

• public DriveTrain() {} is a function that we will use to set the initial state of the items within our “Subsystem” class. This type of function is referred to as a constructor. “Constructor” refers to a function that “constructs” and instance of a class based on the parameters provided to it. We will explain more about constructors and instances before we move on to the “Commands” section.

• One thing we will want to do in the constructor for this “DriveTrain” class is invert one of the motors, if we have two motors on our robot for driving the left and right wheels, we need to invert one of the motors so that the wheels spin in the same direction.
  • It sounds kinda confusing but let’s just think about the actual physical robot for a second. We are using the same type of motor on both sides, but we need to rotate the motor so that we can attach it to the wheel on the other side. This means that unless we invert one of the motors, the wheels will turn opposite directions.
    • We can invert the motor using one of PWMSparkMax’s “methods” (functions inside of a class that are made available to us). This method and instructions on how to use it can be found in the documents.

      

      The method “expects” a “boolean” type as an argument. A “boolean” is just a data type that stores whether something is “true” or “false”. Because we want the right motor to be inverted, we “pass” this method a boolean value of “true” in the call to the function.

      rightMotor.setInverted(true)

      • What is an “argument”?

        A value provided by the user at the time a function is called. Here are two function calls that supply different arguments.

        • add(1,2) . This will return 3 as the sum.

        • add(456,1024)
  • Example of our DriveTrain() constructor method

    public Drivetrain(){
    	rightMotor.setInverted(true)
    }

• public Periodic() {} I don’t know what this is useful for yet, but we need to have it there even if it is empty. It’s part of the template.

• Now that we have 2 motors defined in our system, we can now define our own “method” in this class that can be used to drive the robot forward. This will become useful when we create our own “command” later on. Methods are the way that we allow people to use the black box that we are creating.

  • Let’s call this method “forward”.

  • We want this method to take one parameter that corresponds with the speed the motors should turn forward.

  • The PWMSparkMax class has a “method” that wee need to use in order to set the speed that the motor is turning. It expects a value between -1.0 and 1.0
    • With that in mind, our function definition will look like this

      public void forward(double speed){}

      • What is a double ?

        A double is a data type that can hold a decimal. This is in contrast to an int or “integer” that can only represent whole numbers like 1, 2, 3, ...

        Below is the same number “1” in two different forms:

        int : 1

        double : 1.0

        The value of something like 0.9 or 6.8 cannot be held in an int type, but a double can be used instead

    • it is public because we want it to be available or “exposed” for outside code to use, and void, because we are not returning anything, we are just setting the speed of the motors.

  • In the “body” of the function, we will set the speed of the left motor and the right motor to the value that was “passed” to us in the function. This is the double speed we just talked about.

  • ONE MORE IMPORTANT STEP, we want to make sure that the value we were given is a positive value and that it fits in the range from -1.0 to 1.0. If we don’t, we may have an unexpected result or potentially damage our robot.

    Consider what might happen if code outside this class calls this function in this way forward(-1.0). The function will set the motors to run in reverse at full speed. Not exactly what we would want to happen when we call a method called “forward”.

    Now consider what might happen if forward(10.0) is called. If there aren’t any other safeguards, we might break something on our robot because the value is too high.

    • Here is the complete example including the implementation of forward(double speed).

      // Copyright (c) FIRST and other WPILib contributors.
      // Open Source Software; you can modify and/or share it under the terms of
      // the WPILib BSD license file in the root directory of this project.
      
      package frc.robot.subsystems;
      
      import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
      import edu.wpi.first.wpilibj2.command.SubsystemBase;
      
      public class DriveTrain extends SubsystemBase {
        /** Creates a new DriveTrain. */
        private final PWMSparkMax leftMotor = new PWMSparkMax(Constants.LEFT_MOTOR_PORT);
        private final PWMSparkMax rightMotor = new PWMSparkMax(Constants.RIGHT_MOTOR_PORT);
      
        public DriveTrain() {
          rightMotor.setInverted(true);
        }
      
        // sets the motors to move at a certain speed, so long as it is a positive value between 0 and 1
        public void forward(double speed){
          // before we send this value to the motors we want to only send it if it is a value between 0.0 and 1.0
          // anything else is an invalid value for this function, because we are going FORWARD
          if(speed <= 1.0 && speed > 0.0){
            leftMotor.set(speed);
            rightMotor.set(speed);
          } else{
            // we enter this block if we are given a bad value, let's just set the motors to zero
            leftMotor.set(0.0);
            rightMotor.set(0.0);
          }
        }
      
        @Override
        public void periodic() {
          // This method will be called once per scheduler run
        }
      }

• Let’s define another method called stop() that will set the motors back to 0.0 . We should note that this is exactly the same thing that forward(0.0) would do, but the only way we would know that is if we know how the forward function works. The goal of creating this class is to create a “black box” where outside code can interact with it in terms of “go forward this fast” and “stop” instead of in terms of “set motors to this value”. Notice how outside users don’t need to know very much about how the internals work, they just need to tell it when to go forward and when to stop.

  • In an effort to help you learn, I suggest you try to make this function on your own. It is exactly the same as the function we made before, except we will set the motor values to 0.0 every time this function is called instead of the double speed value that we were using before. I have put the completed example of this subsystem at the end, but again, I highly recommend you try this on your own.

  • Here is the complete example WITHOUT the stop() method/function. Use this as your base. I’d love to help you if you need assistance.

    // Copyright (c) FIRST and other WPILib contributors.
    // Open Source Software; you can modify and/or share it under the terms of
    // the WPILib BSD license file in the root directory of this project.
    
    package frc.robot.subsystems;
    
    import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
    import edu.wpi.first.wpilibj2.command.SubsystemBase;
    
    public class DriveTrain extends SubsystemBase {
      /** Creates a new DriveTrain. */
      private final PWMSparkMax leftMotor = new PWMSparkMax(Constants.LEFT_MOTOR_PORT);
      private final PWMSparkMax rightMotor = new PWMSparkMax(Constants.RIGHT_MOTOR_PORT);
    
      public DriveTrain() {
        rightMotor.setInverted(true);
      }
    
      // sets the motors to move at a certain speed, so long as it is a positive value between 0 and 1
      public void forward(double speed){
        // before we send this value to the motors we want to only send it if it is a value between 0.0 and 1.0
        // anything else is an invalid value for this function, because we are going FORWARD
        if(speed <= 1.0 && speed > 0.0){
          leftMotor.set(speed);
          rightMotor.set(speed);
        } else{
          // we enter this block if we are given a bad value, let's just set the motors to zero
          leftMotor.set(0.0);
          rightMotor.set(0.0);
        }
      }
    
      // function that stops the motors
      public void stop(){
        // implement the stop() function here
      }
    
      @Override
      public void periodic() {
        // This method will be called once per scheduler run
      }
    }

• Here is a completed example of the “DriveTrain” subsystem with our forward(double speed) and stop() methods fully implemented:

  // Copyright (c) FIRST and other WPILib contributors.
  // Open Source Software; you can modify and/or share it under the terms of
  // the WPILib BSD license file in the root directory of this project.
  
  package frc.robot.subsystems;
  
  import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
  import edu.wpi.first.wpilibj2.command.SubsystemBase;
  
  public class DriveTrain extends SubsystemBase {
    /** Creates a new DriveTrain. */
    private final PWMSparkMax leftMotor = new PWMSparkMax(Constants.LEFT_MOTOR_PORT);
    private final PWMSparkMax rightMotor = new PWMSparkMax(Constants.RIGHT_MOTOR_PORT);
  	
  	// this is the function that sets the defaults when this class is created
    public DriveTrain() {
      rightMotor.setInverted(true);
    }
  
    // sets the motors to move at a certain speed, so long as it is a positive value between 0 and 1
    public void forward(double speed){
      // before we send this value to the motors we want to only send it if it is a value between 0.0 and 1.0
      // anything else is an invalid value for this function, because we are going FORWARD
      if(speed <= 1.0 && speed > 0.0){
        leftMotor.set(speed);
        rightMotor.set(speed);
      } else{
        // we enter this block if we are given bad values, let's just set the motors to zero
        leftMotor.set(0.0);
        rightMotor.set(0.0);
      }
    }
  
    // function that stops the motors
    public void stop(){
      leftMotor.set(0.0);
      rightMotor.set(0.0);
    }
  
    @Override
    public void periodic() {
      // This method will be called once per scheduler run
    }
  }

“RobotContainer” Class

• Now is a good time to take another look at the diagram of the system that we are creating, so that we can see what we have covered so far and how it fits into the larger system

  /*
   *                                                                    ┌─────────────────────┐
   *                                                                    │Physical Button Input│
   *                                                                    └───────┬─────────────┘
   *                                                                            │
   *                                                                            │
   * ┌────────────────────────┬───────────────┬─────────────────────────────────┼──────────────┐
   * │                        │Robot Container│                                 │              │
   * │                        └───────────────┘                                 │              │
   * │  ┌───────────────────┐                                          ┌────────▼───────┐      │
   * │  │DriveTrain Instance│                                          │Button Variable │      │
   * │  └─────┬─────────────┘                                          └────────────────┘      │
   * │        │                                                                                │
   * │        │  ┌────────────────────┬───────────────────────────────────────────────────┐    │
   * │        │  │DriveForward Command│                                                   │    │
   * │        │  ├────────────────────┘                                                   │    │
   * │        │  │                                              ┌─────┐     ┌────────┐    │    │
   * │        │  │                                              │Speed│     │Duration│    │    │
   * │        │  │                                              └──┬──┘     └───┬────┘    │    │
   * │        │  │     ┌────────────────────────────────────────┬──▼────────────▼─┐       │    │
   * │        └──┼─────►DriveForward's Local DriveTrain Variable│                 │       │    │
   * │           │     ┌────────────────────────────────────────┘                 │       │    │
   * │           │     │                                                          │       │    │
   * │           │     │                                                          │       │    │
   * │           │     │                                  ┌───────────┬───────┐   │       │    │
   * │           │     │                                  │PWMSparkMax│       │   │       │    │
   * │           │     │                                  ├───────────┘       │   │       │    │
   * │           │     │                                  │                   │   │       │    │
   * │           │     │                                  │                   │   │       │    │
   * │           │     │                                  │                   │   │       │    │
   * │           │     │                                  └──────────┬────────┘   │       │    │
   * │           │     │                                             │            │       │    │
   * │           │     └─────────────────────────────────────────────┼────────────┘       │    │
   * │           │                                                   │                    │    │
   * │           │                                                   │                    │    │
   * │           └───────────────────────────────────────────────────┼────────────────────┘    │
   * │                                                               │                         │
   * │                                                               │                         │
   * └───────────────────────────────────────────────────────────────┼─────────────────────────┘
   *                                                                 │
   *                                                                 │
   *                                                                 │
   *                                                      ┌──────────▼─────────┐
   *                                                      │Physical Motor Speed│
   *                                                      └────────────────────┘
   */
  /* needs to show that the local drivetrain variable is assigned THROUGH the use of the constructor for that 
   class and show what other methods do as well*/

• Why are we covering the “RobotContainer” before the “Command” class?
  • “RobotContainer” is the class that directly uses the “Command” class. By understanding the purpose of “RobotContainer” first, we can better understand why the “Command” class is structured the way it is.
  • In short, the purpose of the “Command” class is to provide a set of instructions to a subsystem or set of subsystems. It does this by using the methods that the subsystems make available for use.
    • the public forward(double speed) function in our “DriveTrain” subsystem is an example of a method that the “DriveForward” command might use.

• Constructors
  • As we mentioned earlier, constructors are functions that create instances of classes. But we didn’t fully explain what an “instance” is.

• Variables vs. Instances
  • “PWMSparkMax” is another class that has its own constructor. We just used this constructor when we wrote new PWMSparkMax(port number). This bit of code creates a new instance of the “PWMSparkMax” class based on the port number that we provided to it.
  • If we only wrote private final PWMSparkMax rightMotor; without using = new PWMSparkMax(1); we would have declared an variable rightMotor that could hold an instance of the “PWMSparkMax”, but does not yet. At some later point we could give it an instance by writing rightMotor =new PWMSparkMax(1);. There are reasons for doing this that we will cover in the section covering “Commands”.
  • The same effect can be seen if we declared a int variable with the line int number; .

    At this point an integer variable called “number” exists, but it does not contain a value until we assign one to it with a line like number = 3;

  • Bottom line:
    • Variable - A place that can hold an instance and a name that can be used to access the data inside the instance
    • Instance - section of data created by a constructor for a class
  • ⬆️ This concept is important and tricky. If you don’t feel like you could explain it yourself, please ask me to clarify

• Here is what the empty template for the “RobotContainer” class looks like. It comes with an example command and example subsystem namedm_exampleSubsystem. They don’t do anything in this file except show us how we should define our commands and subsystems in this file.

  // Copyright (c) FIRST and other WPILib contributors.
  // Open Source Software; you can modify and/or share it under the terms of
  // the WPILib BSD license file in the root directory of this project.
  
  package frc.robot;
  
  import edu.wpi.first.wpilibj.GenericHID;
  import frc.robot.commands.ExampleCommand;
  import frc.robot.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 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;
    }
  }

• The job of the RobotContainer is to:
  1. Use the constructor for each subsystem to create a single instance of our subsystem(s).
  2. Use each command’s constructor to create an instance of our command(s)
     • Steps 1 and 2 are written in the section with the comment // The robot's subsystems and commands are defined here...
  3. Supply the Command(s) that we create with the inputs they need
     1. Joystick Values
     2. Button Values
     3. Instance of a Subsystem it needs to use.
     4. Other values
     • This is done in the configureButtonBindings() method in this class

• So why did we cover the distinction between variables and instances in the section about the “RobotContainer”?
  • I’m about to drop a bit of a bombshell on you: Our overall robot code will be able to run more than one “Command” at the same time.

    PleaseDon’tPanicIt’sGonnaBeOk. We don’t need to worry about how this is accomplished, it’s just one of those black boxes we can take for granted as long as we follow the rules I set out in this section.

  • Because multiple commands can be run at the same time, we need a way of making sure that any subsystem can only be used by one command at a time. If we don’t protect our subsystems, then 2 commands running at the same time could tell the motors to turn forward and to stop at the same time.
  • We can protect our subsystems by:
    1. Only declaring one instance of each subsystem in “RobotContainer”
    2. Passing that same instance into the constructor of each command that needs it
    3. Using a function in each command that makes sure it can have control of this instance of the subsystem before it starts telling the subsystem to do things. We will cover how that works in the next section.
  • This is what it looks like to create a single instance of a subsystem:

    private final DriveTrain m_drivetrain = new DriveTrain()

  • This is what it looks like to pass this same instance to the constructor of a Command that needs to use this subsystem.

    private final DriveForward m_driveforward = new DriveForward(m_drivetrain)

    Doing this ⬆️ will ensure that the same instance is passed to each command and we will be protected.

    Doing this ⬇️ will create a new subsystem instance each time and will not protect from 2 commands trying to control the same set of parts at the same time

    // BAD
    private final DriveForward m_driveforward = new DriveForward(new DriveTrain()) // DO NOT USE THIS
    // BAD

• This is what it will look like to declare an instance of our subsystem and our command in the robot container. I know we haven’t covered how to define our Command yet, but hold tight and trust that knowing how this works will help us understand how the “Command” class is written.
  • Here is our RobotContainer with our DriveTrain subsystem and DriveForward command declared. At this point we are referencing the “DriveForward” Command which does not exist yet, so just ignore any errors on that line for now if you are following along in VSCode.

    // Copyright (c) FIRST and other WPILib contributors.
    // Open Source Software; you can modify and/or share it under the terms of
    // the WPILib BSD license file in the root directory of this project.
    
    package frc.robot;
    
    import edu.wpi.first.wpilibj.GenericHID;
    import frc.robot.commands.ExampleCommand;
    import frc.robot.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);
    
    	// follow the declaration of the above Example subsystem and command in order to declare our own subsystem and command
    	private final DriveTrain m_drivetrain = new DriveTrain();
    	// declare a new instance of our command and pass it the instance of the drive train that we just declared in the previous line
    	private final DriveForward m_driveForwardCommand = new DriveForward(m_drivetrain);
    
      /** 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 passing it to a {@link
       * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
       */
      private void configureButtonBindings() {
    		// we will cover this method in the section after the "Command" section
      }
    
      /**
       * 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;
      }
    }

• We still have not covered how to bind buttons and joysticks to Commands (making it so that when I press a button a command will run, etc.). First, we will cover how to make our own Command and then we will revisit this idea.

“Command” Class

• The “Command” class is used to provide a set of instructions to a subsystem or set of subsystems

• Let’s walk through creating a command called “DriveForward” (which tells the robot to drive forward at a certain speed for a certain amount of time) as an example of a command. Let’s walk through how we might implement it using the “Command” class.

• This is an empty template for our “Command” class. By filling in the empty functions provided for us, we will create the “DriveForward” command

  // Copyright (c) FIRST and other WPILib contributors.
  // Open Source Software; you can modify and/or share it under the terms of
  // the WPILib BSD license file in the root directory of this project.
  
  package frc.robot.commands;
  
  import edu.wpi.first.wpilibj2.command.CommandBase;
  
  public class DriveForward extends CommandBase {
    /** Creates a new DriveForward. */
    public DriveForward() {
      // Use addRequirements() here to declare subsystem dependencies.
    }
  
    // Called when the command is initially scheduled.
    @Override
    public void initialize() {}
  
    // Called every time the scheduler runs while the command is scheduled.
    @Override
    public void execute() {}
  
    // Called once the command ends or is interrupted.
    @Override
    public void end(boolean interrupted) {}
  
    // Returns true when the command should end.
    @Override
    public boolean isFinished() {
      return false;
    }
  }

• Just like with our subsystem class there are a couple lines of code importing the packages

• Again, just like our subsystem class, we want to define variables that we can use across any function defined inside of this class.
  • First we will define two variables, one for the speed that we want to drive (between 0 and 1) and one for the duration that we want to drive (number of milliseconds). Notice that the first variable is of type double, because we need a decimal value and the second variable is a new type long. The reason we use milliseconds and the long type is because it is easier to use with the function we use to track the time. We will cover this in the initialize() and isFinished() section.

    private final double speed = 0.1; // 0.0 is the MIN, 1.0 is the MAX speed
    private final long duration = 3000; // number of milliseconds that we want to drive forward, 3 seconds

  • We will next declare a variable that can hold an instance of our DriveTrain “Subsystem” class and we will use this class like a “black box” in our Command. We will assign an instance to it in the constructor for this class, but for now we will just declare the variable.
  • Before we can do this, we need to import the code that we just wrote that defines what a “DriveTrain” is. I can show you how this can be done easily and automatically through VSCode, but for now this is what the line you will need to use looks like. We should place this line alongside the other lines of code that import so that we can see everything we are importing in one place.

    import frc.robot.subsystems.DriveTrain;

  • This is our code now that our DriveTrain class is imported and our variables are declared

    // Copyright (c) FIRST and other WPILib contributors.
    // Open Source Software; you can modify and/or share it under the terms of
    // the WPILib BSD license file in the root directory of this project.
    
    package frc.robot.commands;
    
    import edu.wpi.first.wpilibj2.command.CommandBase;
    import frc.robot.subsystems.DriveTrain;
    
    public class DriveForward extends CommandBase {
      /** Creates a new DriveForward. */
      private final double speed = 0.1; // 0.0 is the MIN, 1.0 is the MAX speed
      private final double duration = 3.0; // number of seconds that we should drive forward
      private final DriveTrain m_drivetrain;
    
      public DriveForward() {
        // Use addRequirements() here to declare subsystem dependencies.
      }
    
      // Called when the command is initially scheduled.
      @Override
      public void initialize() {}
    
      // Called every time the scheduler runs while the command is scheduled.
      @Override
      public void execute() {}
    
      // Called once the command ends or is interrupted.
      @Override
      public void end(boolean interrupted) {}
    
      // Returns true when the command should end.
      @Override
      public boolean isFinished() {
        return false;
      }
    }

• Ok, let’s define our DriveForward’s constructor function DriveForward().
  • For now, all we need to provide our DriveForward() constructor with is our DriveTrain instance that our RobotContainer will provide. Remember this is so that DriveForward can check to make sure it has exclusive control of our DriveTrain instance before it starts telling it to do things. This is what our empty constructor looks like.

    public DriveForward(DriveTrain drivetrain){}

  • What we will do in this constructor is:
    1. Take the DriveTrain instance passed to us by the RobotContainer and assign it to our “local” variable m_drivetrain so that is can be used in any of the methods in this class.

       m_drivetrain = drivetrain;

    2. Use the addRequirements() function to tell the rest of our system that this command requires exclusive control of this subsystem before it start to tell it to do things. We don’t need to know how this function works, we just need to provide it a subsystem and trust that it performs that function, because it says so in the documentation of the code.

       Now that we have completed the step before this one, m_drivetrain and drivetrain refer to the same “DriveTrain” instance. So the writing addRequirements(drivetrain) would have the same effect as writing addRequirements(m_drivetrain). Just an interesting thing to note here.

       addRequirements(m_drivetrain)

  • This is what our finished constructor looks like:

    public DriveForward(DriveTrain drivetrain) {
    	// Use addRequirements() here to declare subsystem dependencies.
    	m_drivetrain = drivetrain;
    	addRequirements(m_drivetrain);
    }

• We still have a few functions to fill out in this class. To better understand how these functions will be used, let’s discuss a concept known as a state machine.
  • A state machine is an abstract machine that has collection of “states” that it can inhabit (one at a time) and a set of operations that can move it from one state to another.
  • This might sound complicated, but can actually be quite simple, let’s assume YOU are a simple state machine.
    • Let’s assume you have only two “states” that you can exist in, we will call them call them “happy” and “sad”.
    • You also have only two operations “listen to music” and “stub your toe”.
      1. If you are in the “sad” state, the operation “listen to music” will move you to the “happy” state.
      2. If you are in the “happy” state, the operation “stub your toe” will move you to the “sad” state.
      3. If you “listen to music” while you are in the “happy state” you will continue to be in the “happy” state
      4. If you “stub your toe” while in the sad state, you will continue to be in the “sad state”
    • Here is a visual diagram of the state machine we just described

      

• This command and its functions will be used to help move our robot from state to state in a similar state machine
  • The same as the machine we described before, there are a set number of states and operations
  • States:
    • Idle
    • Initializing
    • Executing
    • Ending
  • Operations
    • Command is triggered
    • IsFinished() returns False
    • IsFinished() returns True
    • interrupted by another triggered command
  • Here is the set of transitions
    1. The robot begins in the IDLE state, when it sees that a command has been triggered, it transitions from the IDLE state to the INITIALIZING state.
    2. When the robot is in the INITIALIZING STATE, the initialize() function for the command that was triggered will be executed. Once it completes, our state machine will automatically transition to the EXECUTING state
    3. When the robot is in the EXECUTING state, it will run the code in execute().
       1. When isFinished() returns False , it will continue to exist in the EXECUTING state
       2. When isFinished() returns True, the machine will transition to the END state
       3. If another command is triggered that needs to use the same subsystem as this command, then the machine will move to the ENDING state.
    4. In the ENDING state, the code inside end() will run once and then the machine will automatically transition to the IDLE state
  • Here is a diagram of this state machine

    

• As you might have seen, there are functions that correspond with the states in this state machine. We should write these functions with the idea of the state machine in our minds. It will also help us to write our other commands.
  • Before we implement any functions, we will want to declare any variables in this class that we can use across all our functions. In this case all we need is a DriveTrain (which we already took care of) and a variable that can be used to keep track of the amount of time that has elapsed. For reasons that will be clear later, this value needs to be a long type, which is basically a big integer.

    private long startTime;

  • initialize()
    • This is what we do before any of the code in execute() is run. In this function we will initialize our timer and we will set our motors to the speed that was provided.
    • In order to keep track of the time, we will will use a function in the “System” library: System.currentTimeMillis(). This function returns the number of milliseconds that have passed since January 1st, 1970. We will save the start time inside of the startTime variable, and then compare the current time to the startTime to see how much time has passed. You will see this happen in the isFinished() function.
  • execute()
    • this is where our main command code would be written if we needed to do anything else. For example there could be code here that checks a proximity sensor to see how close our robot is to the nearest obstacle. It should be noted that any code in this block will be run over and over by the code outside this class.
    • Since we have already turned our motors on in the initialize() function and we plan on turning them off in the end() function, there is nothing we need to do here.
  • end()
    • Before we exit this command in the state machine, we should shut off the motors.
    • There is boolean value interrupted that is passed to this function that will be true if the command ended because it was interrupted. It will be false if it ended because of the isFinished() function. We can use this variable in this function if it matters why this command is ending, but in this case it does not matter. We will just stop the motors no matter what.

      public void end(boolean interrupted) {
      	m_drivetrain.stop();
      }

  • isFinished()
    • Unlike the other functions in this class, this function must return a boolean value of true or false. This corresponds with whether the conditions have been met for the Command to be “finished”. In this case, the only thing that determines whether this command is finished is whether the amount of time that we want it to run has elapsed. We can check this by comparing the current time to the time to startTime. If System.currentTimeMillis() - startTime is greater than duration, then this Command is finished.

“RobotContainer” Class (continued...)

• So, before we move forward, let’s review. At this point we have a...
  1. Subsystem with 2 motors
  2. Command that can tell a DriveTrain Subsystem to start moving forward, wait for 3 seconds, and then tell a DriveTrain Subsystem to stop
  3. RobotContainer that manages the relationship between our drivetrain and our command

• We are very close to being done with this system, but we still need a way to “trigger” the command. What we want in this case is for a button to trigger this command to run.
  • What we need to do is “bind” a button to our DriveForward command so that it will run when we press it. We can do this in our RobotContainer’s configureButtonBindings() method.
  • WPI Library has a class for controllers. the “Joystick” class can be used to access many types of controllers and their buttons/joystick values.

    private final Joystick xboxController = new Joystick(0); // declare a new xbox controller with the port it is connected to

  • We can instead store the 0 value in the "Constants" class under the name XBOX_CONTROLLER_PORT the same way we did the motor ports, but for now I will keep it simple.
  • Next, we store the Xbox controller’s “A” button in its own class, so that we can later bind it to a command. We need to know the index of the “A” button on the Xbox controller. I learned through trial and error running the code that the index was 1. The button indexing starts at 1, so this is the first button.

    private final JoystickButton aButton = new JoystickButton(xboxController, 1)

  • Inside the configureButtonBindings() method, we bind the button we created to our m_driveForward command. The whenPressed() function will trigger this command when the “A” button is pressed on the controller. In order to trigger the command again, you will need to release the button and press it again.

    There are other functions that are part of the “JoystickButton” class that will activate commands continuously while you hold the button and so on...

    
    private void configureButtonBindings() {
    	aButton.whenPressed(m_driveForward);
    }

• This is the completed “RobotContainer” file.

  // Copyright (c) FIRST and other WPILib contributors.
  // Open Source Software; you can modify and/or share it under the terms of
  // the WPILib BSD license file in the root directory of this project.
  
  package frc.robot;
  
  import edu.wpi.first.wpilibj.GenericHID;
  import edu.wpi.first.wpilibj.XboxController;
  import frc.robot.commands.DriveForward;
  import frc.robot.commands.ExampleCommand;
  import frc.robot.subsystems.DriveTrain;
  import frc.robot.subsystems.ExampleSubsystem;
  import edu.wpi.first.wpilibj2.command.Command;
  import edu.wpi.first.wpilibj2.command.button.JoystickButton;
  
  /**
   * 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);
  
    private final DriveTrain m_drivetrain = new DriveTrain();
    private final DriveForward m_driveForward = new DriveForward(m_drivetrain);
    
    private final XboxController xboxController = new XboxController(0);
    private final JoystickButton aButton = new JoystickButton(xboxController, 0); //not sure if zero is a proper button index
  
    /** 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 passing it to a {@link
     * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
     */
    private void configureButtonBindings() {
      aButton.whenPressed(m_driveForward);
    }
  
    /**
     * 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;
    }
  }

Completed Project Files (I have tested them on the Robo Romi)

Deploying Our Code...

I don’t think I have time for this, put some resources here about loading up the driver’s station and adding joysticks...

Further Reading

• Programming with the Robo Romi

• WPI Library Documentation

• FIRST’s official “Zero to Robot” guide

• W3schools tutorials on Java Classes (focus on the first 9 sections)

• Codecademy Java (sections 1-3 are esp. important)

• Guide to “Imaging” your RoboRIO

• FIRST’s Command-Based programming guide