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.
Like we did for the driveForTime command we will create this command in our DriveMechanism.java file by adding the following function:
|
1 2 3 4 5 |
public Command arcadeDrive() { return run(coroutine -> { }).named("arcadeDrive"); } |
Now, what we want to do is to use the Joystick to control the two motors that will let us drive the robot arcade style. To do this, we are going to need access to the joystick. We could just pass in the entire Joystick class so that we have access but this would require our command to have knowledge about the type of joystick we are using and if we decided to switch to a Gamepad we would need to change our function. Instead we are going to only pass in two functions that will allow us to read the current X and Y values from the Joystick. To get access to these functions we will change the declaration of our function as follows:
|
1 2 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { .... |
Now we are ready to implement the run() function. What we want to do is to create an infinite loop where we will read the current values from the joystick and set the motor power accordingly. So for the run() function we are going to want to do something like:
|
1 2 3 4 5 6 7 8 9 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick // Set motor power } }).named("arcadeDrive"); } |
Now there is one very important thing that is missing here. The system is designed so that multiple commands can run simultaneously and if we create an infinite loop like this run() will never return and this will hang your entire robot. What we need to do is to provide a way for other things to run while we are in this loop. They way to do this is to call the function coroutine.yeild() from withing the loop as follows:
|
1 2 3 4 5 6 7 8 9 10 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick // Set motor power coroutine.yield(); } }).named("arcadeDrive"); } |
Now, each time through the loop, we yield control and allow other commands to run. Looking at this code you might be thinking “Isn’t this still an infinite loop? How does this command ever end?“. Yes, it appears to be an infinite loop but calling the yield() function not only allows other commands to run but it also allows this command to be canceled.
Anytime you have a loop in your code you need to have it yield (unless it is a very small and fast loop). One important thing to note is each time you yield, your code will not get another chance to run for 20ms.
Now we are ready to start controlling our robot. Let’s start by just reading the Y value from the joystick and make the robot drive forward and backwards using this value. We can get the Y position of the joystick by calling ySupplier.getAsDouble() and the joystick will return 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 our DriveMechanism. The run() function of your arcadeDrive(…) should now look like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); // Set motor power setPower(y, y); coroutine.yield(); } }).named("arcadeDrive"); } |
And let’s add a whenCanceled(…) to stop the robot when the command is canceled:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); // Set motor power setPower(y, y); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
To have the robot respond to joystick input, we don’t want to connect this command to a button like we did with the driveForTime command. We want this command to always run – to always look for input from the Joystick – whenever no other command that requires the DriveMechanism is running. To do this we add a setDefaultCommand(…) call to the constructor of our TeleopOpCode class as follows:
|
1 2 3 |
robot.m_driveMechanism.setDefaultCommand(robot.m_driveMechanism.arcadeDrive( () -> robot.m_cmdJoystick.getX(), () -> robot.m_cmdJoystick.getY())); |
The odd looking code:
|
1 |
() -> robot.m_cmdJoystick.getX() |
is just a way for us to pass the getX() joystick function to our command. We need to pass the function itself, not just the current value, because the arcadeDrive command will needs to be able to repeatedly call it to get the current value. This is called a lambda expression and you will be using it a lot when programming your robots.
Your TeleopOpCode.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 |
// Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. package first.robot; import org.wpilib.opmode.OpMode; import org.wpilib.opmode.Teleop; import robotCore.Logger; @Teleop public class TeleopOpMode implements OpMode { public TeleopOpMode(Robot robot) { Logger.log("TeleopOpMode", 2, "TeleopOpMode"); robot.m_driveMechanism.setDefaultCommand(robot.m_driveMechanism.arcadeDrive( () -> robot.m_cmdJoystick.getX(), () -> robot.m_cmdJoystick.getY())); robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime(0.75, 2)); } @Override public void start() { Logger.log("TeleopOpMode", 2, "start()"); } } |
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 run() function of our arcadeDrive as follows:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); // Set motor power setPower(x, -x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
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 run() 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 9 10 11 12 13 14 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); // Set motor power setPower(y + x, y - x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
Before we move on we need to address an issue with mechanical joysticks. When you use the virtual joystick the x and y return to exactly zero. However for a real joystick you will find that is not the case. When you let go of the joystick you will see that there usually remains small values for the x and y. To fix this problem we are going to apply a “dead band“, a value below which we will treat the joystick as reading exactly zero. Fortunately WPILib provide a function MathUtil.applyDeadband just for that purpose. Adding this to our function we get:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); x = MathUtil.applyDeadband(x, 0.05); y = MathUtil.applyDeadband(y, 0.05); // Set motor power setPower(y + x, y - x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
With this setting, any value from the joystick whose magnitude is below 0.05 will be treated as zero.
Now 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 run() function as follows:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); x = MathUtil.applyDeadband(x, 0.05); y = MathUtil.applyDeadband(y, 0.05); x = x * x * x; y = y * y * y; // Set motor power setPower(y + x, y - x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
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 28 29 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); x = MathUtil.applyDeadband(x, 0.05); y = MathUtil.applyDeadband(y, 0.05); if (x < 0) { x = -x * x; } else { x = x * x; } if (y < 0) { y = -y * y; } else { y = y * y; } // Set motor power setPower(y + x, y - x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
As a final note, there are other ways this may be accomplished such as 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 13 14 15 16 17 18 19 20 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); x = MathUtil.applyDeadband(x, 0.05); y = MathUtil.applyDeadband(y, 0.05); x = Math.abs(x) * x; y = Math.abs(y) * y; // Set motor power setPower(y + x, y - x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
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 and zero if the number is zero.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
public Command arcadeDrive(DoubleSupplier xSupplier, DoubleSupplier ySupplier) { return run(coroutine -> { while (true) { // Read Joystick double y = ySupplier.getAsDouble(); double x = xSupplier.getAsDouble(); x = MathUtil.applyDeadband(x, 0.05); y = MathUtil.applyDeadband(y, 0.05); x = x * x * Math.signum(x); y = y * y * Math.signum(y); // Set motor power setPower(y + x, y - x); coroutine.yield(); } }).whenCanceled(this::stop).named("arcadeDrive"); } |
These show that there are many correct ways to solve a problem given the tools Java provides…and in general!
Before we go on, try an experiment which will demonstrate how the command requirements work. Note that in order to perform this experiment you will need to be using an actual joystick rather than the virtual one provided by the Driver Station. Move the joystick to the right so that the robot is turning in place and then while holding it there press the trigger (which is button #1 for the joystick). The robot will stop turning and drive forward for 2 seconds, and during that time the joystick will have no effect. After the 2 seconds, the joystick takes over again and the robot will once again start turning.
Here is what happened. Both arcadeDrive and driveForTime require the DriveMechanism. When you pressed the trigger, the scheduler started driveForTime, and since only one command can require the DriveMechanism at a time, it canceled arcadeDrive (which called its whenCanceled and stopped the motors). When driveForTime finished, nothing was using the DriveMechanism anymore, so the scheduler automatically started its default command, arcadeDrive, again.