Speed Control – Robotics Programming

Now that we know how to use the encoders, we want to change our DriveSubsystem class so that we can set the motors to run at a specific speed by automatically adjusting the power of the motors to maintain that speed.

Before we tackle that, however, we need to gather some information as to the relationship between the power applied to the motors and their speed. We are going to create a new command where we will run the robot forward at different power settings and record the speed that results.

Start by adding a new class TestMotorSpeedCommand to the commands folder, and copy the contents of ExampleCommand.java replacing the ExampleCommand and ExampleSubsystem strings. Your result 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.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Logger;

/**
 * Test the maximum drive speed.
 */
public class TestMotorSpeedCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;

  /**
   * Creates a new TestMotorSpeedCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public TestMotorSpeedCommand(DriveSubsystem subsystem) {
    Logger.log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");

    m_subsystem = subsystem;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.log("TestMotorSpeedCommand", 2, "initialize()");
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.log("TestMotorSpeedCommand", -1, "execute()");
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.log("TestMotorSpeedCommand", -1, "isFinished()");
    return false;
  }
}

/*----------------------------------------------------------------------------*/

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

import edu.wpi.first.wpilibj2.command.CommandBase;

import robot.subsystems.DriveSubsystem;

import robotCore.Logger;

/**

* Test the maximum drive speed.

public class TestMotorSpeedCommand extends CommandBase {

private final DriveSubsystem m_subsystem;

/**

* Creates a new TestMotorSpeedCommand.

* @param subsystem The subsystem used by this command.

public TestMotorSpeedCommand(DriveSubsystem subsystem) {

Logger.log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");

m_subsystem = subsystem;

// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_subsystem);

}

// Called when the command is initially scheduled.

@Override

public void initialize() {

Logger.log("TestMotorSpeedCommand", 2, "initialize()");

}

// Called every time the scheduler runs while the command is scheduled.

@Override

public void execute() {

Logger.log("TestMotorSpeedCommand", -1, "execute()");

}

// Called once the command ends or is interrupted.

@Override

public void end(boolean interrupted) {

Logger.log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));

}

// Returns true when the command should end.

@Override

public boolean isFinished() {

Logger.log("TestMotorSpeedCommand", -1, "isFinished()");

return false;

}

Now what we want to do is to start the robot at 0 power and gradually increase the power, recording the speed, until we reach full power. To accomplish this we need to create a member variable m_power to hold the current power setting. We are also going to want to record the speed of the motors so we will need an Encoder instance for the left and right encoders.

public class TestMotorSpeedCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private double m_power;
  private final Encoder m_leftEncoder;
  private final Encoder m_rightEncoder;

  /**
   * Creates a new TestMotorSpeedCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public TestMotorSpeedCommand(DriveSubsystem subsystem) {
    Logger.log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");

    m_subsystem = subsystem;
    m_leftEncoder = subsystem.getLeftEncoder();
    m_rightEncoder = subsystem.getRightEncoder();
    
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

public class TestMotorSpeedCommand extends CommandBase {

private final DriveSubsystem m_subsystem;

private double m_power;

private final Encoder m_leftEncoder;

private final Encoder m_rightEncoder;

/**

* Creates a new TestMotorSpeedCommand.

* @param subsystem The subsystem used by this command.

public TestMotorSpeedCommand(DriveSubsystem subsystem) {

Logger.log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");

m_subsystem = subsystem;

m_leftEncoder = subsystem.getLeftEncoder();

m_rightEncoder = subsystem.getRightEncoder();

// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_subsystem);

}

In the initialize() function we need to set the power to 0. Also we will be logging the motor speeds in the execute() function in a format that will allow us to import them into a spreadsheet. To make the spreadsheet easier to read we add a title row which labels each column.

  public void initialize() {
    Logger.log("TestMotorSpeedCommand", 2, "initialize()");

    Logger.log("TestMotorSpeedCommand", 0, ",Power,Left,Right");

    m_power = 0;
  }

public void initialize() {

Logger.log("TestMotorSpeedCommand", 2, "initialize()");

Logger.log("TestMotorSpeedCommand", 0, ",Power,Left,Right");

m_power = 0;

}

Then in the execute() function we will set the drive power to the current power and then increase the power by 0.0025 each time we are called. We will use the getSpeed() function of the Encoder class to log the speeds of the two motors so that we can view them later. To accomplish this we change the execute() function as follows:

  public void execute() {
    Logger.log("TestMotorSpeedCommand", -1, "execute()");

    Logger.log("TestMotorSpeedCommand", 0, String.format(",%f,%d,%d", m_power, 
                                      m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));

    m_subsystem.setPower(m_power, m_power);

    m_power += 0.0025;
  }

public void execute() {

Logger.log("TestMotorSpeedCommand", -1, "execute()");

Logger.log("TestMotorSpeedCommand", 0, String.format(",%f,%d,%d", m_power,

m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));

m_subsystem.setPower(m_power, m_power);

m_power += 0.0025;

}

Then we need to change the isFinished() function to return true when the power exceeds 1.3. Note that we are increasing the power up to 1.3 even though the highest power the motor will accept is 1.0 (anything over that will be treated the same as 1.0). The reason we are doing this is to see exactly where the speed tops out at.

  public boolean isFinished() {
    Logger.log("TestMotorSpeedCommand", -1, "isFinished()");
    return m_power > 1.3;
  }

public boolean isFinished() {

Logger.log("TestMotorSpeedCommand", -1, "isFinished()");

return m_power > 1.3;

}

Finally when the command is finished, we need to turn the motors off.

  public void end(boolean interrupted) {
    Logger.log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

public void end(boolean interrupted) {

Logger.log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));

m_subsystem.setPower(0, 0);

}

Your TestMotorSpeedCommand.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.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Encoder;
import robotCore.Logger;

/**
 * Test the maximum drive speed.
 */
public class TestMotorSpeedCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private double m_power;
  private final Encoder m_leftEncoder;
  private final Encoder m_rightEncoder;

  /**
   * Creates a new TestMotorSpeedCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public TestMotorSpeedCommand(DriveSubsystem subsystem) {
    Logger.log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");

    m_subsystem = subsystem;
    m_leftEncoder = subsystem.getLeftEncoder();
    m_rightEncoder = subsystem.getRightEncoder();

    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.log("TestMotorSpeedCommand", 2, "initialize()");

    Logger.log("TestMotorSpeedCommand", 0, ",Power,Left,Right");

    m_power = 0;
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.log("TestMotorSpeedCommand", -1, "execute()");

    Logger.log("TestMotorSpeedCommand", 0,
        String.format(",%f,%d,%d", m_power, m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));

    m_subsystem.setPower(m_power, m_power);

    m_power += 0.0025;
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.log("TestMotorSpeedCommand", -1, "isFinished()");
    return m_power > 1.3;
  }
}

/*----------------------------------------------------------------------------*/

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

import edu.wpi.first.wpilibj2.command.CommandBase;

import robot.subsystems.DriveSubsystem;

import robotCore.Encoder;

import robotCore.Logger;

/**

* Test the maximum drive speed.

public class TestMotorSpeedCommand extends CommandBase {

private final DriveSubsystem m_subsystem;

private double m_power;

private final Encoder m_leftEncoder;

private final Encoder m_rightEncoder;

/**

* Creates a new TestMotorSpeedCommand.

* @param subsystem The subsystem used by this command.

public TestMotorSpeedCommand(DriveSubsystem subsystem) {

Logger.log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");

m_subsystem = subsystem;

m_leftEncoder = subsystem.getLeftEncoder();

m_rightEncoder = subsystem.getRightEncoder();

// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_subsystem);

}

// Called when the command is initially scheduled.

@Override

public void initialize() {

Logger.log("TestMotorSpeedCommand", 2, "initialize()");

Logger.log("TestMotorSpeedCommand", 0, ",Power,Left,Right");

m_power = 0;

}

// Called every time the scheduler runs while the command is scheduled.

@Override

public void execute() {

Logger.log("TestMotorSpeedCommand", -1, "execute()");

Logger.log("TestMotorSpeedCommand", 0,

String.format(",%f,%d,%d", m_power, m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));

m_subsystem.setPower(m_power, m_power);

m_power += 0.0025;

}

// Called once the command ends or is interrupted.

@Override

public void end(boolean interrupted) {

Logger.log("TestMotorSpeedCommand", 2, String.format("end(%b)", interrupted));

m_subsystem.setPower(0, 0);

}

// Returns true when the command should end.

@Override

public boolean isFinished() {

Logger.log("TestMotorSpeedCommand", -1, "isFinished()");

return m_power > 1.3;

}

Finally we want to set it so that button 3 on the joystick will run this command. Switching to the RobotContainer.java file, we add a declaration for m_button3:

public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  @SuppressWarnings("unused")
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
  private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
  private final Joystick m_joystick = new Joystick(0);
  private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
  private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
  private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);

public class RobotContainer {

// The robot's subsystems and commands are defined here...

@SuppressWarnings("unused")

private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();

private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();

private final Joystick m_joystick = new Joystick(0);

private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);

private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);

private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);

and then configure it to run our TestMotorSpeedCommand when it is pressed:

  private void configureButtonBindings() {
    m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
    m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));
    m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
  }

private void configureButtonBindings() {

m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));

m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));

m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));

}

Now run your program and connect and enable your robot. Before pressing the B3 button to run our command we want to clear the TERMINAL in VS Code. Unfortunately the only way I have found to do that is a little bit convoluted (if anyone can figure out a simpler way to do this, please let me know!). First right click on the word TERMINAL at the bottom of the screen. This will bring up the following menu:

Now ignore this menu and without closing it move the cursor down to the TERMINAL area and right click. This should then give you the menu:

Choose the Clear option from this menu.

Now press the B3 button on the Driver Station. The robot should drive forward slowly increasing it speed. When it has stopped, the logged information should be in the TERMINAL pane:

Now we want to select all of the text in this window so we can paste it into our spreadsheet. Unfortunately using the normal Windows shortcut CTRL + a does not seem to work for some reason. However we can us the same trick that we used to clear the TERMINAL. First right click on the word TERMINAL. Then, ignoring that menu, right click on the TERMINAL window and choose Select All. Then repeat this procedure and this time choose Copy.

Now that you have the text copied to the clipboard start up LibreOffice Calc. Then choose Edit / Paste and you should get the following dialog:

Make sure the Comma option is checked under the Separator Options and then click OK.

Then select the Power, Left and Right columns (i.e. D, E and F):

and click the Insert Chart icon at the top:

For the chart type choose the XY (Scatter) and Lines Only options as shown below:

Which should produce a graph similar to the one below:

This is a plot of the motor speed vs the power applied. There are several things we can see from looking at the graph. The first is that the motors will not even start running until the power reaches about 0.3 and tops out when the power reaches 1.0 (a power of 1.0 represents running the motor at full power). The second is that the left motor is less powerful than the right which is the reason the robot turns left. The last takeaway is that the max speed for the right motor is about 1200 and the max speed of the left motor is about 1150. If we want the robot to drive straight then the max speed for the whole robot will need to be limited to the slower motor (i.e. 1150). So let’s define a constant for that max speed:

  public static final double k_maxSpeed = 1150;

1	public static final double k_maxSpeed = 1150;

Note that we have made k_maxSpeed public here. This is so we can later use it to log the target speed in the units of the encoders.

Note that these are the results for this particular robot. Your robot may differ and you should adjust these numbers accordingly.

The SmartMotor class (which the PWMMotor class inherits) has a built in mechanism to control the speed of the motor using a PID controller. PID stands for Proportional, Integral, Differential. When we use this control mechanism, we will set a target for the speed we desire (called the setPoint) and the SmartMotor class will adjust the power sent to the motor based on the following formula:

  power = (P * error) + (I * totalError) + (D * deltaError) + (F * setPoint)

1	power = (P * error) + (I * totalError) + (D * deltaError) + (F * setPoint)

where

P – represents the proportional scale factor.
error – represents the difference between the current speed and the setPoint.
I – represents the integral scale factor.
totalError – represents the total accumulated error over time.
D – represents the derivative scale factor.
deltaError – represents the difference between the current error and the last.
F – represents the constant scale factor which is proportional to the setPoint.
setPoint – represents the current target speed.

The trick is picking the right combination of P, I, D and F to make the motor behave like you want.

We will start with the F term which represents a constant power which proportional to the setPoint. Now we know that at full power the maximum speed of the right motor is about 1200units/sec. Thus we need a F term that, when multiplied by the max speed (i.e. 1200) will result in full power being applied (i.e. a power of 1.0). That number is 1.0/1200, which is the number we will use for the right motor. In a similar manner we compute that we should use 1.0/1150 for the left motor. Note that in both expressions, we 1.0 in the numerator rather than just 1. This is because in Java, if an expression contains only integers, then the result will be an integer thus 1/1200expressed as an integer is just zero.

Note if we use only the constant term, this will essentially be the same as controlling the motor by power alone. If there is more or less resistance to the motors, no adjustments will occur and the motors will slow down or speed up. However, we want to get this term set so that the motors run close to the target speed we desire.

To set the F terms, we add following two lines to the DriveSubsystem constructor:

    m_leftMotor.setFTerm(k_Fleft);
    m_rightMotor.setFTerm(k_Fright);

1 2	m_leftMotor.setFTerm(k_Fleft); m_rightMotor.setFTerm(k_Fright);

Of course we must define k_Fleft and k_Fright:

  private static final double k_Fleft = 1.0/k_maxSpeed;
  private static final double k_Fright = 1.0/k_maxSpeed;

1 2	private static final double k_Fleft = 1.0/k_maxSpeed; private static final double k_Fright = 1.0/k_maxSpeed;

Also, in order for the motor controller to control the speed of the motors, it will need to know what encoders are being used to measure the speed. We set that by calling setFeedbackDevice() for the motors in the DriveSubsystem constructor as follows:

    m_leftMotor.setFeedbackDevice(m_leftEncoder);
    m_rightMotor.setFeedbackDevice(m_rightEncoder);

1 2	m_leftMotor.setFeedbackDevice(m_leftEncoder); m_rightMotor.setFeedbackDevice(m_rightEncoder);

Next we want to set a scale factor so when we want to set the speed, we can just use a range from -1.0 to +1.0 like we do with other motors. Otherwise we would need to set the speed based on the arbitrary units of the encoder. We do this by calling setMaxSpeed() in the constructor:

m_leftMotor.setMaxSpeed(k_maxSpeed);
m_rightMotor.setMaxSpeed(k_maxSpeed);

1 2	m_leftMotor.setMaxSpeed(k_maxSpeed); m_rightMotor.setMaxSpeed(k_maxSpeed);

Finally, we could change the setPower function of our DriveSubsystem class to set the speed instead, but there may be a time in the future where we still want to control the power. Therefore, we will create a new function setSpeed which we will use to set the speed as follows:

  public void setSpeed(double leftSpeed, double rightSpeed) {
    m_leftMotor.setControlMode(SmartMotorMode.Speed);
    m_rightMotor.setControlMode(SmartMotorMode.Speed);

    m_leftMotor.set(leftSpeed);
    m_rightMotor.set(rightSpeed);
  }

public void setSpeed(double leftSpeed, double rightSpeed) {

m_leftMotor.setControlMode(SmartMotorMode.Speed);

m_rightMotor.setControlMode(SmartMotorMode.Speed);

m_leftMotor.set(leftSpeed);

m_rightMotor.set(rightSpeed);

}

Note that we must ensure that the controller is in speed mode by calling the setControlMode() function. Then we set the desired speeds.

We also need to change the setPower function to set the control mode back to power by adding the following to setPower:

  public void setPower(double leftPower, double rightPower) {
    m_leftMotor.setControlMode(SmartMotorMode.Power);
    m_rightMotor.setControlMode(SmartMotorMode.Power);

    m_rightMotor.set(rightPower);
    m_leftMotor.set(leftPower);
  }

public void setPower(double leftPower, double rightPower) {

m_leftMotor.setControlMode(SmartMotorMode.Power);

m_rightMotor.setControlMode(SmartMotorMode.Power);

m_rightMotor.set(rightPower);

m_leftMotor.set(leftPower);

}

Your DriveSubsystem.java file should now look something 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.Device;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;

public class DriveSubsystem extends SubsystemBase {
  private static final int k_leftMotorPWMPin = Device.M1_1_DIR;
  private static final int k_leftMotorDirPin = Device.M1_2_DIR;
  private static final int k_rightMotorPWMPin = Device.M1_1_PWM;
  private static final int k_rightMotorDirPin = Device.M1_1_DIR;
  private static final int k_leftEncoderPin1 = Device.Q1_INT;
  private static final int k_leftEncoderPin2 = Device.Q1_DIR;
  private static final int k_rightEncoderPin1 = Device.Q2_INT;
  private static final int k_rightEncoderPin2 = Device.Q2_DIR;

  public static final double k_maxSpeed = 1150;

  private static final double k_Fleft = 1.0 / k_maxSpeed;
  private static final double k_Fright = 1.0 / k_maxSpeed;

  private PWMMotor m_leftMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
  private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
  private Encoder m_leftEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_leftEncoderPin1,
      k_leftEncoderPin2);
  private Encoder m_rightEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_rightEncoderPin1,
      k_rightEncoderPin2);

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

    m_leftMotor.setFeedbackDevice(m_leftEncoder);
    m_rightMotor.setFeedbackDevice(m_rightEncoder);
    m_leftMotor.setMaxSpeed(k_maxSpeed);
    m_rightMotor.setMaxSpeed(k_maxSpeed);

    m_leftMotor.setFTerm(k_Fleft);
    m_rightMotor.setFTerm(k_Fright);

    m_rightEncoder.setInverted(true);
  }

  public void setPower(double leftPower, double rightPower) {
    m_leftMotor.setControlMode(SmartMotorMode.Power);
    m_rightMotor.setControlMode(SmartMotorMode.Power);

    m_rightMotor.set(rightPower);
    m_leftMotor.set(leftPower);
  }

  public void setSpeed(double leftSpeed, double rightSpeed) {
    m_leftMotor.setControlMode(SmartMotorMode.Speed);
    m_rightMotor.setControlMode(SmartMotorMode.Speed);

    m_leftMotor.set(leftSpeed);
    m_rightMotor.set(rightSpeed);
  }

  public Encoder getLeftEncoder() {
    return (m_leftEncoder.clone());
  }

  public Encoder getRightEncoder() {
    return (m_rightEncoder.clone());
  }

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

/*----------------------------------------------------------------------------*/

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

import robotCore.Encoder;

import robotCore.Logger;

import robotCore.PWMMotor;

import robotCore.SmartMotor.SmartMotorMode;

public class DriveSubsystem extends SubsystemBase {

private static final int k_leftMotorPWMPin = Device.M1_1_DIR;

private static final int k_leftMotorDirPin = Device.M1_2_DIR;

private static final int k_rightMotorPWMPin = Device.M1_1_PWM;

private static final int k_rightMotorDirPin = Device.M1_1_DIR;

private static final int k_leftEncoderPin1 = Device.Q1_INT;

private static final int k_leftEncoderPin2 = Device.Q1_DIR;

private static final int k_rightEncoderPin1 = Device.Q2_INT;

private static final int k_rightEncoderPin2 = Device.Q2_DIR;

public static final double k_maxSpeed = 1150;

private static final double k_Fleft = 1.0 / k_maxSpeed;

private static final double k_Fright = 1.0 / k_maxSpeed;

private PWMMotor m_leftMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);

private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);

private Encoder m_leftEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_leftEncoderPin1,

k_leftEncoderPin2);

private Encoder m_rightEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_rightEncoderPin1,

k_rightEncoderPin2);

/**

* Creates a new DriveSubsystem.

public DriveSubsystem() {

Logger.log("DriveSubsystem", 3, "DriveSubsystem()");

m_leftMotor.setFeedbackDevice(m_leftEncoder);

m_rightMotor.setFeedbackDevice(m_rightEncoder);

m_leftMotor.setMaxSpeed(k_maxSpeed);

m_rightMotor.setMaxSpeed(k_maxSpeed);

m_leftMotor.setFTerm(k_Fleft);

m_rightMotor.setFTerm(k_Fright);

m_rightEncoder.setInverted(true);

}

public void setPower(double leftPower, double rightPower) {

m_leftMotor.setControlMode(SmartMotorMode.Power);

m_rightMotor.setControlMode(SmartMotorMode.Power);

m_rightMotor.set(rightPower);

m_leftMotor.set(leftPower);

}

public void setSpeed(double leftSpeed, double rightSpeed) {

m_leftMotor.setControlMode(SmartMotorMode.Speed);

m_rightMotor.setControlMode(SmartMotorMode.Speed);

m_leftMotor.set(leftSpeed);

m_rightMotor.set(rightSpeed);

}

public Encoder getLeftEncoder() {

return (m_leftEncoder.clone());

}

public Encoder getRightEncoder() {

return (m_rightEncoder.clone());

}

@Override

public void periodic() {

// This method will be called once per scheduler run

Logger.log("DriveSubsystem", -1, "periodic()");

}

Next we want to add a new command that we can use to test out our speed control. Create a new class under the commands folder called CalibrateSpeedCommand and, as before, copy over the code from ExampleCommand, renaming the the file name, and replacing ExampleCommand and ExampleSubsystem as you have done before.

Now for this test, we are simply going to run the robot at a constant speed and log the results so that we can see how close to that speed we come. Since we will be logging the motor speeds, we will need access to the left and right encoders so we declare member variables to hold those values and initialize them in the constructor:

public class CalibrateSpeedCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private final double k_speed = 0.60;

  private Encoder m_leftEncoder;
  private Encoder m_rightEncoder;

  /**
   * Creates a new CalibrateSpeedCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public CalibrateSpeedCommand(DriveSubsystem subsystem) {
    Logger.log("CalibrateSpeedCommand", 3, "CalibrateSpeedCommand()");

    m_subsystem = subsystem;
    m_leftEncoder = m_subsystem.getLeftEncoder();
    m_rightEncoder = m_subsystem.getRightEncoder();
    
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

public class CalibrateSpeedCommand extends CommandBase {

private final DriveSubsystem m_subsystem;

private final double k_speed = 0.60;

private Encoder m_leftEncoder;

private Encoder m_rightEncoder;

/**

* Creates a new CalibrateSpeedCommand.

* @param subsystem The subsystem used by this command.

public CalibrateSpeedCommand(DriveSubsystem subsystem) {

Logger.log("CalibrateSpeedCommand", 3, "CalibrateSpeedCommand()");

m_subsystem = subsystem;

m_leftEncoder = m_subsystem.getLeftEncoder();

m_rightEncoder = m_subsystem.getRightEncoder();

// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_subsystem);

}

In the initialize() function we will simply use our new setSpeed(…) function to start the robot forward at 0.60 speed. Since we will also be logging the speed, we log the title row for our CSV file.

  public void initialize() {
    Logger.log("CalibrateSpeedCommand", 2, "initialize()");

    Logger.log("CalibrateSpeedCommand", 3, "...,Target Speed,Left Speed, Right Speed");
        
    m_subsystem.setSpeed(k_speed,  k_speed);

  }

public void initialize() {

Logger.log("CalibrateSpeedCommand", 2, "initialize()");

Logger.log("CalibrateSpeedCommand", 3, "...,Target Speed,Left Speed, Right Speed");

m_subsystem.setSpeed(k_speed, k_speed);

}

And where we have defined k_speed above.

In the execute() function we want to log the current time, power, and speeds of the two motors.

  public void execute() {
    Logger.log("CalibrateSpeedCommand", -1, "execute()");

    Logger.log("TestSpeedControl", 3, String.format(",%.0f,%d,%d", k_speed * DriveSubsystem.k_maxSpeed,
                                                                        m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));
  }

public void execute() {

Logger.log("CalibrateSpeedCommand", -1, "execute()");

Logger.log("TestSpeedControl", 3, String.format(",%.0f,%d,%d", k_speed * DriveSubsystem.k_maxSpeed,

m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));

}

Note that we are also logging the target speed and we need to multiply k_speed by the k_maxSpeed defined in the DriveSubsystem so that the units will match those returned by the encoder’s getSpeed() function.

Now for this command we are going to make it run as long as we hold down button 4 on the joystick. Since the command will never end on it’s own, we want to always return false from the isFinished() function.

When we release button 4 on the joystick, this command will be interrupted and it’s end() function will be called. At that point we need to turn the motors off.

  public void end(boolean interrupted) {
    Logger.log("CalibrateSpeedCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

public void end(boolean interrupted) {

Logger.log("CalibrateSpeedCommand", 2, String.format("end(%b)", interrupted));

m_subsystem.setPower(0, 0);

}

Your CalibrateSpeedCommand.java 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.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Encoder;
import robotCore.Logger;

/**
 * Calibrates the speed by setting the F, P, I and IZone values.
 */
public class CalibrateSpeedCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private final double k_speed = 0.60;

  private Encoder m_leftEncoder;
  private Encoder m_rightEncoder;

  /**
   * Creates a new CalibrateSpeedCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public CalibrateSpeedCommand(DriveSubsystem subsystem) {
    Logger.log("CalibrateSpeedCommand", 3, "CalibrateSpeedCommand()");

    m_subsystem = subsystem;
    m_leftEncoder = m_subsystem.getLeftEncoder();
    m_rightEncoder = m_subsystem.getRightEncoder();

    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.log("CalibrateSpeedCommand", 2, "initialize()");

    Logger.log("CalibrateSpeedCommand", 3, "...,Target Speed,Left Speed, Right Speed");

    m_subsystem.setSpeed(k_speed, k_speed);

  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.log("CalibrateSpeedCommand", -1, "execute()");

    Logger.log("TestSpeedControl", 3, String.format(",%.0f,%d,%d", k_speed * DriveSubsystem.k_maxSpeed,
        m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.log("CalibrateSpeedCommand", 2, String.format("end(%b)", interrupted));

    m_subsystem.setPower(0, 0);
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.log("CalibrateSpeedCommand", -1, "isFinished()");
    return false;
  }
}

/*----------------------------------------------------------------------------*/

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

import edu.wpi.first.wpilibj2.command.CommandBase;

import robot.subsystems.DriveSubsystem;

import robotCore.Encoder;

import robotCore.Logger;

/**

* Calibrates the speed by setting the F, P, I and IZone values.

public class CalibrateSpeedCommand extends CommandBase {

private final DriveSubsystem m_subsystem;

private final double k_speed = 0.60;

private Encoder m_leftEncoder;

private Encoder m_rightEncoder;

/**

* Creates a new CalibrateSpeedCommand.

* @param subsystem The subsystem used by this command.

public CalibrateSpeedCommand(DriveSubsystem subsystem) {

Logger.log("CalibrateSpeedCommand", 3, "CalibrateSpeedCommand()");

m_subsystem = subsystem;

m_leftEncoder = m_subsystem.getLeftEncoder();

m_rightEncoder = m_subsystem.getRightEncoder();

// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_subsystem);

}

// Called when the command is initially scheduled.

@Override

public void initialize() {

Logger.log("CalibrateSpeedCommand", 2, "initialize()");

Logger.log("CalibrateSpeedCommand", 3, "...,Target Speed,Left Speed, Right Speed");

m_subsystem.setSpeed(k_speed, k_speed);

}

// Called every time the scheduler runs while the command is scheduled.

@Override

public void execute() {

Logger.log("CalibrateSpeedCommand", -1, "execute()");

Logger.log("TestSpeedControl", 3, String.format(",%.0f,%d,%d", k_speed * DriveSubsystem.k_maxSpeed,

m_leftEncoder.getSpeed(), m_rightEncoder.getSpeed()));

}

// Called once the command ends or is interrupted.

@Override

public void end(boolean interrupted) {

Logger.log("CalibrateSpeedCommand", 2, String.format("end(%b)", interrupted));

m_subsystem.setPower(0, 0);

}

// Returns true when the command should end.

@Override

public boolean isFinished() {

Logger.log("CalibrateSpeedCommand", -1, "isFinished()");

return false;

}

Now connect our test button 4 to our new CalibrateSpeedCommand by adding a m_button4 to RobotContainer:

public class RobotContainer {
  // The robot's subsystems and commands are defined here...
  @SuppressWarnings("unused")
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
  private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
  private final Joystick m_joystick = new Joystick(0);
  private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
  private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
  private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);
  private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);

public class RobotContainer {

// The robot's subsystems and commands are defined here...

@SuppressWarnings("unused")

private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();

private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();

private final Joystick m_joystick = new Joystick(0);

private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);

private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);

private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);

private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);

And configuring it so that our new command will run while the button his held down:

  private void configureButtonBindings() {
    m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
    m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));
    m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
    m_button4.whileHeld(new CalibrateSpeedCommand(m_driveSubsystem));
  }

private void configureButtonBindings() {

m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));

m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));

m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));

m_button4.whileHeld(new CalibrateSpeedCommand(m_driveSubsystem));

}

Your RobotContainer.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;

import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import robot.commands.ArcadeDriveCommand;
import robot.commands.CalibrateSpeedCommand;
import robot.commands.DriveForDistanceCommand;
import robot.commands.DriveForTimeCommand;
import robot.commands.ExampleCommand;
import robot.commands.TestMotorSpeedCommand;
import robot.subsystems.DriveSubsystem;
import robot.subsystems.ExampleSubsystem;
import robotCore.Joystick;

/**
 * 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...
  @SuppressWarnings("unused")
  private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
  private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
  private final Joystick m_joystick = new Joystick(0);
  private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);
  private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);
  private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);
  private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);

  private final ExampleCommand m_autoCommand = null;

  /**
   * The container for the robot. Contains subsystems, OI devices, and commands.
   */
  public RobotContainer() {
    m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick));

    // 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() {
    m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));
    m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));
    m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));
    m_button4.whileHeld(new CalibrateSpeedCommand(m_driveSubsystem));
  }

  /**
   * 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;
  }
}

/*----------------------------------------------------------------------------*/

/* 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;

import edu.wpi.first.wpilibj2.command.Command;

import edu.wpi.first.wpilibj2.command.button.JoystickButton;

import robot.commands.ArcadeDriveCommand;

import robot.commands.CalibrateSpeedCommand;

import robot.commands.DriveForDistanceCommand;

import robot.commands.DriveForTimeCommand;

import robot.commands.ExampleCommand;

import robot.commands.TestMotorSpeedCommand;

import robot.subsystems.DriveSubsystem;

import robot.subsystems.ExampleSubsystem;

import robotCore.Joystick;

/**

* 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...

@SuppressWarnings("unused")

private final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();

private final DriveSubsystem m_driveSubsystem = new DriveSubsystem();

private final Joystick m_joystick = new Joystick(0);

private final JoystickButton m_button1 = new JoystickButton(m_joystick, 1);

private final JoystickButton m_button2 = new JoystickButton(m_joystick, 2);

private final JoystickButton m_button3 = new JoystickButton(m_joystick, 3);

private final JoystickButton m_button4 = new JoystickButton(m_joystick, 4);

private final ExampleCommand m_autoCommand = null;

/**

* The container for the robot. Contains subsystems, OI devices, and commands.

public RobotContainer() {

m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick));

// 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() {

m_button1.whenPressed(new DriveForTimeCommand(m_driveSubsystem, 0.75, 3.0));

m_button2.whenPressed(new DriveForDistanceCommand(m_driveSubsystem, 0.75, 4000));

m_button3.whenPressed(new TestMotorSpeedCommand(m_driveSubsystem));

m_button4.whileHeld(new CalibrateSpeedCommand(m_driveSubsystem));

}

/**

* 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;

}

Now run your program, and connect and enable your robot. Then, as before, clear the TERMINAL window in VS Code. Then switch to the Driver Station and hold the B4 button down for a few seconds. Go back to VS Code and copy all of the text in the TERMINAL window and paste it into LibreOffice Calc. As before, delete all of the rows up to the header line. Now select the Target Speed, Left Speed, and Right Speed columns and plot the data. This time, however, choose the Line / Lines Only option as shown below:

This should result in a graph which looks something like this:

The blue line represents the target speed and the red and yellow lines are the actual speeds of the motors. We can see from the graph that the motor speeds are too low so we will need to increase the F term for both motors. To get an approximation of how much we need to change them we can multiply the current value by the ratio of the target speed to the actual speed. We can then multiply that by the current F values to get new estimated F values. We can see that the target speed is 690 and for both motors the current speed is around 450. This means we need to multiply the current F values by 690/450 or 1.5:

  private static final double k_Fleft = 1.5/1500;    // Use the correct number for your robot
  private static final double k_Fright = 1.5/1700;   // Use the correct number for your robot

1 2	private static final double k_Fleft = 1.5/1500; // Use the correct number for your robot private static final double k_Fright = 1.5/1700; // Use the correct number for your robot

Remember that these numbers are for this particular robot only. Your robot may behave slightly differently and you should adjust your number accordingly.

Now run your program again and copy the data to LibreOffice Calc and you should get a graph something like:

This is now a bit high so we are going to reduce the F terms a little bit. Keep adjusting the F values until the motors are running close to the target speed. Don’t worry about getting it exact, the other terms will compensate for small errors. Here are the numbers I used for my robot:

  private static final double k_Fleft = 1.3/1500;    // Use the correct number for your robot
  private static final double k_Fright = 1.3/1700;   // Use the correct number for your robot

1 2	private static final double k_Fleft = 1.3/1500; // Use the correct number for your robot private static final double k_Fright = 1.3/1700; // Use the correct number for your robot

With these numbers I got the graph:

Now this is much better. Remember that we do not need this to be perfect. In fact it is pretty much impossible to get it perfect using only the F term, that is what the other terms are for. But do notice that even now the robot is driving a lot straighter than it was before. Also, note that I ended up using the same value of F for both the left and right motors. You may need to use different values for your robot.

Now that we have the F term established we are going to work on the P term. The P stands for proportional. What this means is that power is added or subtracted depending on how far off the actual speed is from the target speed. Determining the first guess for the P term is somewhat difficult so we are just going to try something and see how it works. Let’s try 0.0002 (in this case we won’t need to use different values for the two motors).

To set the P term we add the following to the constructor:

    m_leftMotor.setPTerm(k_P);
    m_rightMotor.setPTerm(k_P);

1 2	m_leftMotor.setPTerm(k_P); m_rightMotor.setPTerm(k_P);

And defining k_P as follows:

   private static final double k_P = 0.0002;       // Use the correct number for your robot

1	private static final double k_P = 0.0002; // Use the correct number for your robot

Using this value, we get the following graph:

We can see that this graph already looks better than the previous one that did not have a P term. Now what we want is the largest value of P that does not make the robot unstable. If P is too large, the robot will start to oscillate and we don’t want that. So what we are going to do is keep increasing the value of P until we see it become unstable, and then back off.

If we try a P value to 0.005 we will get the following graph:

In my case, this makes the speed unstable as you can see. In your case, this might not happen at a value of 0.005 and if it doesn’t, you should continue to increase the P value until it does become unstable and then revert back to the last stable value. In my case I will be using 0.002 for my value of P

Now adding the P term helps a lot but if the robot were to encounter some additional resistance (such as going up a ramp) it would not be able to completely compensate. For that we need the I term.

The I stands for integral. What the PID controller does is run in a loop. Each time through the loop it computes an error term which is the difference between the set point and the actual speed. The P term then multiplies that error term to make it’s adjustment. However, the PID loop also keeps track of a total error, and each time through the loop the current error term is added to the total error. The I term is then multiplied by the total error and that is added to the power equation. So, lets say, that the speed is too low because the robot is going up a ramp. As long as the speed is too slow, the total error will continue to increase which will cause the power applied to continue to increase until there is enough power to maintain the correct speed.

Once again choosing an initial value for the I term is difficult and we will have to make a guess. Let’s start with 0.001. We set the I term like this::

    m_leftMotor.setITerm(k_I);
    m_rightMotor.setITerm(k_I);

1 2	m_leftMotor.setITerm(k_I); m_rightMotor.setITerm(k_I);

Defining k_I as follows:

  private static final double k_I = 0.0002;

1	private static final double k_I = 0.0002;

There is one last thing we need to do before we run our program. Using the I term is great but really only works when our speed is close to the target value. When we first start the robot, it takes some time for the robot to get up to speed and during that time we build up a large total error term. This then causes us to overshoot our target. Eventually it settles down to the correct value but we don’t want the overshoot. The way to prevent this is to only use the I term when the current speed is close to the target speed. We set this range by using the setIZone() function. Now we need to set the zone large enough that it encompasses the natural oscillation of the motors. Looking at the graphs we can see that this range is around 150. Set the I Zone as follows:

    m_leftMotor.setIZone(k_IZone);
    m_rightMotor.setIZone(k_IZone);

1 2	m_leftMotor.setIZone(k_IZone); m_rightMotor.setIZone(k_IZone);

Defining k_IZone as:

  private static final double k_IZone = 150;

1	private static final double k_IZone = 150;

Now if we run the program we get the following graph:

This looks pretty good. Now we want to do the same thing we did with the P term and keep increasing the value until the robot becomes unstable. For my robot, I found that a P value of 0.002 produced the following.

We can see that we have introduced some stability so I set the I term back to 0.001. Not that for your robot you may need a different I term.

Your DriveSubsystem.java file should now look something 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.Device;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;

public class DriveSubsystem extends SubsystemBase {
  private static final int k_leftMotorPWMPin = Device.M1_1_DIR;
  private static final int k_leftMotorDirPin = Device.M1_2_DIR;
  private static final int k_rightMotorPWMPin = Device.M1_1_PWM;
  private static final int k_rightMotorDirPin = Device.M1_1_DIR;
  private static final int k_leftEncoderPin1 = Device.Q1_INT;
  private static final int k_leftEncoderPin2 = Device.Q1_DIR;
  private static final int k_rightEncoderPin1 = Device.Q2_INT;
  private static final int k_rightEncoderPin2 = Device.Q2_DIR;

  public static final double k_maxSpeed = 1150;

  private static final double k_Fleft = 1.33 / k_maxSpeed;
  private static final double k_Fright = 1.31 / k_maxSpeed;
  private static final double k_P = 0.0002; // Use the correct number for your robot
  private static final double k_I = 0.001;
  private static final double k_IZone = 150;

  private PWMMotor m_leftMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
  private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
  private Encoder m_leftEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_leftEncoderPin1,
      k_leftEncoderPin2);
  private Encoder m_rightEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_rightEncoderPin1,
      k_rightEncoderPin2);

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

    m_leftMotor.setFeedbackDevice(m_leftEncoder);
    m_rightMotor.setFeedbackDevice(m_rightEncoder);
    m_leftMotor.setMaxSpeed(k_maxSpeed);
    m_rightMotor.setMaxSpeed(k_maxSpeed);

    m_leftMotor.setFTerm(k_Fleft);
    m_rightMotor.setFTerm(k_Fright);
    m_leftMotor.setPTerm(k_P);
    m_rightMotor.setPTerm(k_P);
    m_leftMotor.setITerm(k_I);
    m_rightMotor.setITerm(k_I);
    m_leftMotor.setIZone(k_IZone);
    m_rightMotor.setIZone(k_IZone);

    m_rightEncoder.setInverted(true);
  }

  public void setPower(double leftPower, double rightPower) {
    m_leftMotor.setControlMode(SmartMotorMode.Power);
    m_rightMotor.setControlMode(SmartMotorMode.Power);

    m_rightMotor.set(rightPower);
    m_leftMotor.set(leftPower);
  }

  public void setSpeed(double leftSpeed, double rightSpeed) {
    m_leftMotor.setControlMode(SmartMotorMode.Speed);
    m_rightMotor.setControlMode(SmartMotorMode.Speed);

    m_leftMotor.set(leftSpeed);
    m_rightMotor.set(rightSpeed);
  }

  public Encoder getLeftEncoder() {
    return (m_leftEncoder.clone());
  }

  public Encoder getRightEncoder() {
    return (m_rightEncoder.clone());
  }

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

/*----------------------------------------------------------------------------*/

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

import robotCore.Encoder;

import robotCore.Logger;

import robotCore.PWMMotor;

import robotCore.SmartMotor.SmartMotorMode;

public class DriveSubsystem extends SubsystemBase {

private static final int k_leftMotorPWMPin = Device.M1_1_DIR;

private static final int k_leftMotorDirPin = Device.M1_2_DIR;

private static final int k_rightMotorPWMPin = Device.M1_1_PWM;

private static final int k_rightMotorDirPin = Device.M1_1_DIR;

private static final int k_leftEncoderPin1 = Device.Q1_INT;

private static final int k_leftEncoderPin2 = Device.Q1_DIR;

private static final int k_rightEncoderPin1 = Device.Q2_INT;

private static final int k_rightEncoderPin2 = Device.Q2_DIR;

public static final double k_maxSpeed = 1150;

private static final double k_Fleft = 1.33 / k_maxSpeed;

private static final double k_Fright = 1.31 / k_maxSpeed;

private static final double k_P = 0.0002; // Use the correct number for your robot

private static final double k_I = 0.001;

private static final double k_IZone = 150;

private PWMMotor m_leftMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);

private PWMMotor m_rightMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);

private Encoder m_leftEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_leftEncoderPin1,

k_leftEncoderPin2);

private Encoder m_rightEncoder = new Encoder(robotCore.Encoder.EncoderType.Quadrature, k_rightEncoderPin1,

k_rightEncoderPin2);

/**

* Creates a new DriveSubsystem.

public DriveSubsystem() {

Logger.log("DriveSubsystem", 3, "DriveSubsystem()");

m_leftMotor.setFeedbackDevice(m_leftEncoder);

m_rightMotor.setFeedbackDevice(m_rightEncoder);

m_leftMotor.setMaxSpeed(k_maxSpeed);

m_rightMotor.setMaxSpeed(k_maxSpeed);

m_leftMotor.setFTerm(k_Fleft);

m_rightMotor.setFTerm(k_Fright);

m_leftMotor.setPTerm(k_P);

m_rightMotor.setPTerm(k_P);

m_leftMotor.setITerm(k_I);

m_rightMotor.setITerm(k_I);

m_leftMotor.setIZone(k_IZone);

m_rightMotor.setIZone(k_IZone);

m_rightEncoder.setInverted(true);

}

public void setPower(double leftPower, double rightPower) {

m_leftMotor.setControlMode(SmartMotorMode.Power);

m_rightMotor.setControlMode(SmartMotorMode.Power);

m_rightMotor.set(rightPower);

m_leftMotor.set(leftPower);

}

public void setSpeed(double leftSpeed, double rightSpeed) {

m_leftMotor.setControlMode(SmartMotorMode.Speed);

m_rightMotor.setControlMode(SmartMotorMode.Speed);

m_leftMotor.set(leftSpeed);

m_rightMotor.set(rightSpeed);

}

public Encoder getLeftEncoder() {

return (m_leftEncoder.clone());

}

public Encoder getRightEncoder() {

return (m_rightEncoder.clone());

}

@Override

public void periodic() {

// This method will be called once per scheduler run

Logger.log("DriveSubsystem", -1, "periodic()");

}

Remember that the actual values for the F, I, and P terms for your robot may differ from the ones shown here.

Before we wrap up, let’s see how it behaves at another speed. Change the k_speed constant in CalibrateSpeedCommand make a graph for 0.75 speed. You should see something like:

We can see that it looks pretty good. There is a bit of an overshoot at the beginning but it is not significant so we will not muck with the PID values. The fact is, no single set of PID terms is going to work for all speeds. When tuning your robot you should tune it for the speed that is most important. You should then check other speeds and adjust the PID parameters only if there is a serious problem.

Let’s make one last change before we move on. Change the call to setPower in the ArcadeDrive() function to setSpeed. This will allow us to control the robot by speed even during teleop which can make it easier to control your robot, especially if the two motors run at different speeds for the same power setting. Your ArcadeDriveCommand.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.commands;

import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Joystick;
import robotCore.Logger;

/**
 * An example command that uses an example subsystem.
 */
public class ArcadeDriveCommand extends CommandBase {
  private final DriveSubsystem m_subsystem;
  private final Joystick m_joystick;

  /**
   * Creates a new ArcadeDriveCommand.
   *
   * @param subsystem The subsystem used by this command.
   */
  public ArcadeDriveCommand(DriveSubsystem subsystem, Joystick joystick) {
    Logger.log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");

    m_subsystem = subsystem;
    m_joystick = joystick;
    // Use addRequirements() here to declare subsystem dependencies.
    addRequirements(m_subsystem);
  }

  // Called when the command is initially scheduled.
  @Override
  public void initialize() {
    Logger.log("ArcadeDriveCommand", 2, "initialize()");
  }

  // Called every time the scheduler runs while the command is scheduled.
  @Override
  public void execute() {
    Logger.log("ArcadeDriveCommand", -1, "execute()");

    double y = m_joystick.getY();
    double x = m_joystick.getX();

    x = x * x * x;
    y = y * y * y;

    m_subsystem.setSpeed(y + x, y - x);
  }

  // Called once the command ends or is interrupted.
  @Override
  public void end(boolean interrupted) {
    Logger.log("ArcadeDriveCommand", 2, String.format("end(%b)", interrupted));
  }

  // Returns true when the command should end.
  @Override
  public boolean isFinished() {
    Logger.log("ArcadeDriveCommand", -1, "isFinished()");
    return false;
  }
}

/*----------------------------------------------------------------------------*/

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

import edu.wpi.first.wpilibj2.command.CommandBase;

import robot.subsystems.DriveSubsystem;

import robotCore.Joystick;

import robotCore.Logger;

/**

* An example command that uses an example subsystem.

public class ArcadeDriveCommand extends CommandBase {

private final DriveSubsystem m_subsystem;

private final Joystick m_joystick;

/**

* Creates a new ArcadeDriveCommand.

* @param subsystem The subsystem used by this command.

public ArcadeDriveCommand(DriveSubsystem subsystem, Joystick joystick) {

Logger.log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");

m_subsystem = subsystem;

m_joystick = joystick;

// Use addRequirements() here to declare subsystem dependencies.

addRequirements(m_subsystem);

}

// Called when the command is initially scheduled.

@Override

public void initialize() {

Logger.log("ArcadeDriveCommand", 2, "initialize()");

}

// Called every time the scheduler runs while the command is scheduled.

@Override

public void execute() {

Logger.log("ArcadeDriveCommand", -1, "execute()");

double y = m_joystick.getY();

double x = m_joystick.getX();

x = x * x * x;

y = y * y * y;

m_subsystem.setSpeed(y + x, y - x);

}

// Called once the command ends or is interrupted.

@Override

public void end(boolean interrupted) {

Logger.log("ArcadeDriveCommand", 2, String.format("end(%b)", interrupted));

}

// Returns true when the command should end.

@Override

public boolean isFinished() {

Logger.log("ArcadeDriveCommand", -1, "isFinished()");

return false;

}

Next: Drive Straight