Drive Subsystem

In the last chapter we learned how to run our program, but it really didn’t do anything other that print logging information. That is because we have not told it to do anything yet. In this chapter we will create code which can be used to control the two drive motors.

To do this we will create a Subsystem. Subsystems are designed to control some aspect of the robot, in this case the drive motors. We are going to need to be able to control the motors using a number of different commands and the advantage of using a Subsystem is that it will enable us to control which command has access to the drive motors at what times.

Rather than create a new subsystem from scratch, we are going to copy the existing ExampleSubsystem. Open the subsystems folder in the panel on the left. Right click on the ExampleSubsystem.java entry and choose Copy. Then click on the subsystems folder, right click and choose Paste. This should result in something like this:

DriveSubsystem1

First we need to change the name of the file. Right click on the ‘ExampleSubsystem copy.java‘ entry on the left choose Rename and rename the file to DriveSubsystem.java. Now click in the pane on the right and choose Edit / Replace from the menu. Then enter ExampleSubsystem as the search text and DriveSubsystem as the replacement. Then click the Replace All button.

DriveSubsystem2

At this point your DriveSubsystem.java file should look like:

/*----------------------------------------------------------------------------*/
/* 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 robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import robotCore.Logger;

public class DriveSubsystem extends SubsystemBase {
  /**
   * Creates a new DriveSubsystem.
   */
  public DriveSubsystem() {
    Logger.Log("DriveSubsystem", 3, "DriveSubsystem()");
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    Logger.Log("DriveSubsystem", -1, "periodic()");
  }
}

The first function is the constructor for the class. The second function periodic will be called periodically as long as your program is running. You can use this function to perform actions that this subsystem might require. We will not actually be using this feature in this tutorial.

So what resources are we going to need? We are going to want to control the left and right motors so we will need some variable to do that. The yellow motors attached to our robot require the PWMMotor class for control. The first thing we must do, then, is to create the variables that will be used to control these motors. Add the following two lines to the top of your DriveSubsystem class:

  private PWMMotor m_leftMotor;
  private PWMMotor m_rightMotor;

When you do this, you will notice that PWMMotor is underlined in red. This means that there is an error here which must be fixed. If you hover the mouse over it you will get the notice ‘PWMMotor cannot be resolved to a type’. The problem here is that we must import the module where this class is defined. We could go to the top of the file and add the required import but there is an easier way.

You will also note that we are prepending the variable names with an ‘m_‘ to identify them as member variables of our class. Later you will see that we also use the prefix ‘k_‘ to identify constants.

If you click on one of the PWMMotor declarations and press CTRL + . (i.e. hold the CTRL key and press the period), it will give you a list of possible fixes:

PWMFix

We should choose the first option.

We now see that the variable names (e.g. m_leftMotor) are now underlined in yellow. The yellow underline represents a warning and while warnings will not prevent your program from running they do point to possible problems that may need to be fixed. In this case if we hover over the warning we will see the message ‘The value of the field DriveSubsystem.m_leftMotor is not used’. This is ok at this time since we will be adding code which will use these fields and the warning will go away. In general, you should try and eliminate all warnings from your program. If you have a warning that you know that you want to ignore you can make it go away using the @SuppressWarnings directive. You should, of course, only suppress warnings that you know for a fact are not going to cause problems.

We have declared the variables to control the motors, but we still need to initialize them. We need to be careful when we create the instances of these controllers. When writing code to control hardware, you cannot expect to be able to communicate with that hardware before the robot is initialized so we must make sure that the motor classes are not instantiated until the robot code has been initialized. However, if we wait to create an instance of this class until the robot is initialized, then it will be safe to initialize these variable in this class’s constructor.

Looking at the documentation for PWMMotor we see the constructor requires 2 integers, a pwmPin and a dirPin. The first pin will control the power of the motor and the second will control the direction. We use a Arduino to provide these signals. For an Arduino, only some of the pins can be configured for PWM (Pulse Width Modulation) output. We will be using the PWM pin 5 for the right motor and PWM pin 6 for the left motor. To control the direction, we will be using pin 4 for the right motor and pin 7 for the left. Given this, we can initialize our motor variables as follows:

public class DriveSubsystem extends SubsystemBase {
  private static final int k_rightMotorPWMPin = 5;
  private static final int k_rightMotorDirPin = 4;
  private static final int k_leftMotorPWMPin = 6;
  private static final int k_leftMotorDirPin = 7;

  private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
  private PWMMotor m_leftMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);

Now that we have defined and initialized the variables that control the motors we need to provide a function that will allow users of this class the ability to set the power on the motors. We will create a function called setPower which will allow us to set the power of the left and right motors. Once again consulting the documentation for PWMMotor we find that we can set the power on an individual motor by calling it’s set(double power) function. This function takes a single parameter which specifies the power and can range from -1.0 for full reverse to +1.0 for full forward. Given this we will define our setPower function as follows.

  public void setPower(double leftPower, double rightPower) {
    m_rightMotor.set(rightPower);
    m_leftMotor.set(leftPower);
  }

Your DriveSubsystem.java file should now look like:

/*----------------------------------------------------------------------------*/
/* 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 robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;
import robotCore.Logger;
import robotCore.PWMMotor;

public class DriveSubsystem extends SubsystemBase {
  private static final int k_rightMotorPWMPin = 5;
  private static final int k_rightMotorDirPin = 4;
  private static final int k_leftMotorPWMPin = 6;
  private static final int k_leftMotorDirPin = 7;

  private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
  private PWMMotor m_leftMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);

  /**
   * Creates a new DriveSubsystem.
   */
  public DriveSubsystem() {
    Logger.Log("DriveSubsystem", 3, "DriveSubsystem()");
  }

  public void setPower(double leftPower, double rightPower) {
    m_rightMotor.set(rightPower);
    m_leftMotor.set(leftPower);
  }

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    Logger.Log("DriveSubsystem", -1, "periodic()");
  }
}

Next: Commands