Arcade Drive
Now that we know how to create commands, let’s do something a little more useful. In this chapter we are going to create a command which will allow us to control the drive motors using a joystick.
The first step is to create the new class called ArcadeDriveCommand under the commands folder. Like we did with the DriveSubsystem, copy the ExampleCommand and paste it into the commands folder. Then change the name of the copy to ArcadeDriveCommand.java and in that file search and replace all instances of ExampleCommand with ArcadeDriveCommand. Also, since this command will be using the DriveSubsystem, replace all instances of ExampleSubsystem with DriveSubsystem. After you have done that you ArcadeDriveCommand.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.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import robot.subsystems.DriveSubsystem;
import robotCore.Logger;
/**
* An example command that uses an example subsystem.
*/
public class ArcadeDriveCommand extends CommandBase {
private final DriveSubsystem m_subsystem;
/**
* Creates a new ArcadeDriveCommand.
*
* @param subsystem The subsystem used by this command.
*/
public ArcadeDriveCommand(DriveSubsystem subsystem) {
Logger.Log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");
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("ArcadeDriveCommand", 2, "initialize()");
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
Logger.Log("ArcadeDriveCommand", -1, "execute()");
}
// 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;
}
}
Now since we are going to control the robot with the joystick, we are going to need access to the Joystick class. We have already created an instance in the RobotContainer and we do not want to create a second instance hear so we will pass in the Joystick instance via the constructor:
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);
}
Note that as we are doing with the DriveSubsystem parameter we have created a member variable m_joystick an initialized it in the constructor with the joystick argument so that we will have access to the Joystick class for the life of this object.
Now in the execute() function we need to read the joystick and set the power on the motors. Let’s start by just reading the Y value from the joystick, and make the robot drive forward and backwards using this value. The function to get the Y position of the joystick is getY() and we note that it returns a number from -1.0 to +1.0. Since the motors take their power setting in the same range, we can simply pass this on to the setPower(…) function of our DriveSubsystem class:
public void execute() {
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
m_subsystem.setPower(y, y);
}
Your ArcadeDriveCommand.java file should now look like this:
/*----------------------------------------------------------------------------*/
/* 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();
m_subsystem.setPower(y, y);
}
// 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;
}
}
Now we don’t want to connect this command to a button like we did with the DriveForTimeCommand. What we want is for this command to always run whenever no other command that requires the DriveSubsystem is running. To do this we add a setDefaultCommand(…) call to the constructor of our RobotContainer class:
public RobotContainer() {
m_driveSubsystem.setDefaultCommand(new ArcadeDriveCommand(m_driveSubsystem, m_joystick));
// Configure the button bindings
configureButtonBindings();
}
Here we are creating an instance of our ArcadeDriveCommand class and setting it as the default command for the DriveSubsystem. With this set, the ArcadeDriveCommand will run whenever no other command that requires the DriveSubsystem is running. For example, if we were to now press the B1 button the ArcadeDriveCommand would be interrupted and the DriveForTimeCommand would execute. Once the DriveForTimeCommand ends, the ArcadeDriveCommand would be restarted and the robot would once again be under joystick control.
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.DriveForTimeCommand;
import robot.commands.ExampleCommand;
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 ExampleCommand m_autoCommand = null; // new ExampleCommand(m_exampleSubsystem);
/**
* 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));
}
/**
* 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 you should be able to use the joystick to move the robot forward and backwards.
Of course, we are going to want to be able to turn our robot. How are we going to do that? To make the robot turn right, we want to run the left motor forward and the right motor backwards. To turn left we need to do the opposite. If we are going to use the X value from the joystick, we might accomplish this by changing the execute() function of our ArcadeDriveCommand class as follows:
public void execute() {
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
double x = m_joystick.getX();
m_subsystem.setPower(x, -x);
}
Now this will enable the robot to turn, but it will no longer drive forward and backward. See if you can figure out how to change the execute() function to accomplish both driving forward and backward and turning. When you have your solution, compare it to the one below:
.
.
.
.
@Override
public void execute() {
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
double x = m_joystick.getX();
m_subsystem.setPower(y + x, y - x);
}
You might find that controlling the robot, especially at low speeds is a bit tricky. There is a simple way that we can improve it. Right now if we were to graph the power we apply to the motors vs the X or Y of the joystick, we would just see a straight line. What we would like to do is curve the relationship so that we have more control over the lower speeds. We can do this by cubing the X and Y values. Compare the linear (red) and cubic (blue) curves:
Notice that in both cases, we can still get full power out of the robot by pushing the joystick full over in one direction or another (this is because 1.0 * 1.0 * 1.0 is still equal to 1.0). However for joystick positions less than 1.0, we see that the power output increases much more slowly than the linear case, allowing for better control at the lower speeds. We can implement this change in our execute() function as follows:
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.setPower(y + x, y - x);
}
Try it out and see if you don’t have better control over the robot.
Note that we could also just square the inputs which would result in a less drastic flattening of the curve. However if we wanted to use the square, we would need to take care to make sure that the sign was correct when X or Y becomes negative (remember that -1.0 * -1.0 is equal to +1.0). As an exercise, why don’t you see if you can come up with a solution that squares the inputs but still preserves the sign. You can then compare your solution to the one below:
.
.
.
.
public void execute()
{
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
double x = m_joystick.getX();
if (x < 0)
{
x = -x * x;
}
else
{
x = x * x;
}
if (y < 0)
{
y = -y * y;
}
else
{
y = y * y;
}
m_subsystem.setPower(y + x, y - x);
}