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 (make sure the copy is in 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:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 |
/*----------------------------------------------------------------------------*/ /* 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 frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class ArcadeDriveCommand extends Command { 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 CommandJoystick class. We have already created an instance in the RobotContainer and we do not want to create a second instance here so we will pass in the CommandJoystick instance via the constructor:
Just like when we added a CommandJoystick reference in RobotContainer, add the import line for it using the “hover” trick.
Also, as we are doing with the DriveSubsystem parameter we have created a member variable m_joystick and initialized it in the constructor with the joystick argument so that we will have access to the CommandJoystick class for the life of this object.
Your code should look like this:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 |
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class ArcadeDriveCommand extends Command { private final DriveSubsystem m_subsystem; private final CommandJoystick m_joystick; /** * Creates a new ArcadeDriveCommand. * * @param subsystem The subsystem used by this command. */ public ArcadeDriveCommand(DriveSubsystem subsystem, CommandJoystick joystick) { Logger.log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()"); m_subsystem = subsystem; m_joystick = joystick; // Use addRequirements() here to declare subsystem dependencies. addRequirements(m_subsystem); } |
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:
|
1 2 3 4 5 6 7 |
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:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 |
/*----------------------------------------------------------------------------*/ /* 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 frc.robot.commands; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.subsystems.DriveSubsystem; import robotCore.Logger; /** * An example command that uses an example subsystem. */ public class ArcadeDriveCommand extends Command { private final DriveSubsystem m_subsystem; private final CommandJoystick m_joystick; /** * Creates a new ArcadeDriveCommand. * * @param subsystem The subsystem used by this command. */ public ArcadeDriveCommand(DriveSubsystem subsystem, CommandJoystick 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; } } |
To have the robot respond to joystick “stick” input, we don’t want to connect this command to a button like we did with the DriveForTimeCommand. We want this command to always run – to always look for input from “stick” – 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:
|
1 2 3 4 5 6 |
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:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 |
/*----------------------------------------------------------------------------*/ /* 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 frc.robot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import frc.robot.commands.DriveForTimeCommand; import frc.robot.commands.ArcadeDriveCommand; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.ExampleSubsystem; /** * 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 CommandJoystick m_joystick = new CommandJoystick(0); private final ArcadeDriveCommand 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_joystick.button(1).onTrue(new DriveForTimeCommand(m_driveSubsystem, 0.50, 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:
|
1 2 3 4 5 6 7 8 |
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:
Don’t scroll down until you have tried to find your own solution!
.
.
.
|
1 2 3 4 5 6 7 8 |
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:
|
1 2 3 4 5 6 7 8 9 10 11 |
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:
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 |
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); } |
As a final note, there is another way this may be accomplished by using the Math.abs function which returns the absolute value of a number.
|
1 2 3 4 5 6 7 8 9 10 11 12 |
public void execute() { Logger.log("ArcadeDriveCommand", -1, "execute()"); double y = m_joystick.getY(); double x = m_joystick.getX(); x = Math.abs(x) * x; y = Math.abs(y) * y; m_subsystem.setPower(y + x, y - x); } |
By taking the absolute value and multiplying it by itself, we square the value but retain the sign. While this involves less typing it may or may be less intuitive.
Finally you can also do this using the Math.signum function which returns +1 if the number is positive and -1 if the number is negative.
|
1 2 3 4 5 6 7 8 9 10 11 12 |
public void execute() { Logger.log("ArcadeDriveCommand", -1, "execute()"); double y = m_joystick.getY(); double x = m_joystick.getX(); x = x * x * Math.signum(x); y = y * y * Math.signum(y); m_subsystem.setPower(y + x, y - x); } |
These show that there are many correct ways to solve a problem given the tools Java provides…and in general!