Arcade Drive
Now that we have our DriveSubsystem and know how to use it, let’s create an ArcadeDriveCommand which will allow us to use the joystick to drive the robot.
Please note that the Driver Station will only check for the presence of a joystick when it is started, so you need to plug in the joystick before launching the Driver Station. Also, if you do not have a joystick available, you can check the Enable Virtual Joystick checkbox and you will be presented with a ‘joystick’ that you can control by clicking and dragging with the mouse:
The first step is to create the new class called ArcadeDriveCommand under the commands folder, copy the ExampleCommand into it and replace ExampleCommand with ArcadeDriveCommand. When you have done this, your ArcadeDriveCommand.java file should look like:
package commands;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class ArcadeDriveCommand extends Command
{
public ArcadeDriveCommand()
{
Logger.Log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");
// Use requires() here to declare subsystem dependencies
// requires(Robot.m_exampleSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("ArcadeDriveCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("ArcadeDriveCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("ArcadeDriveCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("ArcadeDriveCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("ArcadeDriveCommand", 2, "interrupted()");
}
}
Now since this command will once again need to use the DriveSubsystem we will require it in our constructor:
public ArcadeDriveCommand()
{
Logger.Log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
We don’t need to do anything for the initialize() function so we will leave that unchanged.
The execute() function is where we are going to want to set the speed and direction of the motors based on the joystick input. Of course, in order to do that we are going to need to be able to get input from the joystick. Looking at the documentation, we see there is a Joystick class which we can use to do that. The first thing to do is to create a variable for the joystick and initialize it, which we do by adding the following to the top of the class:
private Joystick m_joystick = new Joystick(0);
Note that the joystick constructor requires an integer which specifies the joystick number. For the Rasbperry Pi, we only support a single joystick, but retain the parameter for compatibility with the FRC libraries.
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:
protected void execute()
{
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
Robot.m_driveSubsystem.SetPower(y, y);
}
We don’t want this command to ever end, so we are going to continue to have the isFinished() function always return false.
Your ArcadeDriveCommand.java file should now look like:
package commands;
import robot.Robot;
import robotCore.Joystick;
import robotCore.Logger;
import robotWpi.command.Command;
/**
*
*/
public class ArcadeDriveCommand extends Command
{
private Joystick m_joystick = new Joystick(0);
public ArcadeDriveCommand()
{
Logger.Log("ArcadeDriveCommand", 3, "ArcadeDriveCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("ArcadeDriveCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
Robot.m_driveSubsystem.SetPower(y, y);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("ArcadeDriveCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("ArcadeDriveCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("ArcadeDriveCommand", 2, "interrupted()");
}
}
Now we need to somehow get this command to run. We could do what we did with our DriveForTimeCommand and start it at the beginning of Teleop in our teleopInit() function, but there is a better way. What we want is for the robot to remain under the ArcadeDriveCommand as long as it does not have other commands executing that require the DriveSubsystem. Fortunately, there is a simple way to do this.
If you remember when we created the DriveSubsystem we talked a little about the initDefaultCommand() function, saying that we would address that later. Well now is that later. What we can do in this function is to specify what command is to run when there are no other commands running that need this particular subsystem. We do this by adding the following code to initDefaultCommand().
// Set the default command for a subsystem here.
setDefaultCommand(new ArcadeDriveCommand());
Your DriveSubsystem.java file should now look like:
package subsystem;
import robotWpi.command.Subsystem;
import commands.ArcadeDriveCommand;
import robotCore.Logger;
import robotCore.PWMMotor;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_rightMotorPWMPin = 6;
private static final int k_rightMotorDirPin = 7;
private static final int k_leftMotorPWMPin = 5;
private static final int k_leftMotorDirPin = 4;
private PWMMotor m_leftMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
private PWMMotor m_rightMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
// Set the default command for a subsystem here.
setDefaultCommand(new ArcadeDriveCommand());
}
public void SetPower(double leftPower, double rightPower)
{
m_rightMotor.set(rightPower);
m_leftMotor.set(leftPower);
}
}
The final thing we need to do is remove the code from the teleopInit() function of our java class that starts the DriveForTimeCommand since we no longer want that to execute. After doing so, your Robot.java file should look like:
package robot;
import robotWpi.command.Scheduler;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
public class Robot extends IterativeRobot
{
public static ExampleSubsystem m_exampleSubsystem;
public static OI m_OI;
public static DriveSubsystem m_driveSubsystem;
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotiInit()");
m_driveSubsystem = new DriveSubsystem();
m_exampleSubsystem = new ExampleSubsystem();
m_OI = new OI();
}
/*
* Called at the start of autonomous mode
*/
@Override
public void autonomousInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "autonomousInit()");
}
/**
* Called periodically during autonomous
*/
@Override
public void autonomousPeriodic()
{
Logger.Log("Robot", -1, "autonomousPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called at the start of teleop mode
*/
@Override
public void teleopInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "teleopInit()");
}
/**
* Called periodically during operator control
*/
@Override
public void teleopPeriodic()
{
Logger.Log("Robot", -1, "teleopPeriodic()");
Scheduler.getInstance().run();
Sleep(10);
}
/**
* Called a the start of test mode
*/
@Override
public void testInit()
{
Logger.ResetElapsedTime();
Logger.Log("Robot", 2, "testInit()");
}
/**
* Called periodically during test mode
*/
@Override
public void testPeriodic()
{
Logger.Log("Robot", 0, "testPeriodic()");
Sleep(10);
}
/**
* Main program entry point
*
*/
public static void main(String args[])
{
Robot Robot = new Robot();
Robot.Start(args);
}
}
Now deploy and run your program and enable Teleop. You should be able to drive your robot forward and backward using the joystick.
Now, 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:
protected void execute()
{
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
double x = m_joystick.getX();
Robot.m_driveSubsystem.SetPower(x, -x);
}
Of course, 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:
.
.
.
.
protected void execute()
{
Logger.Log("ArcadeDriveCommand", -1, "execute()");
double y = m_joystick.getY();
double x = m_joystick.getX();
Robot.m_driveSubsystem.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 and cubic 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:
protected 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;
Robot.m_driveSubsystem.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:
.
.
.
.
protected 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;
}
Robot.m_driveSubsystem.SetPower(y + x, y - x);
}
Next: Connecting Commands to Joystick Buttons