Arcade Drive
Now that we have the robot driving, let’s see if we can make it do something more useful. Our goal in this next section is to write code that will allow us to use a joystick to drive the robot. In Eclipse, load the DriveSubsystem.java file.
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 thing we need to do is give the drive subsystem access to the joystick. We declare a variable to handle the joystick by adding the following line to the variable declarations (right after the declaration of m_leftMotor).
Joystick m_joystick = null;
As before, we find that doing so introduces an error. Clicking on the error icon, we find that the solution is to import the Joystick class which we do.
Now we need to initialize the joystick by adding the following line to the Init() function
m_joystick = new Joystick(0);
Note that the Joystick class takes a single integer parameter which specifies which joystick. This version of the pi software currently supports only one joystick, but for compatibility with the FRC classes, we must pass a 0 to the constructor.
Finally, we need to add the following function which will allow us set the power on the drive motors based on the position of the joystick:
public void ArcadeDrive()
{
double x = m_joystick.getX();
double y = m_joystick.getY();
x = (x < 0) ? -(x * x) : (x * x);
y = (y < 0) ? -(y * y) : (y * y);
SetPower(y + x, y - x);
}
This function first reads the current joystick x and y positions. Note that the getX and getY joystick functions return a value between -1 to +1.
The next two lines essentially square the return x and y positions (maintain the sign). Doing this will give us better control over the speed at the low end.
Finally we set the power based on the values of x and y. The value of y controls the forward and backward motion of the robot so we want to set both the left and right motors based on that value. To turn, we need to set the left and right motors in opposite directions, and the turn power needs to be controlled by the x value from the joystick.
At this point, our DriveSubsystem.java file should look like:
package subsystem;
import robotCore.I2CPWMMotor;
import robotCore.Joystick;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_leftMotorPin1 = 0;
private static final int k_leftMotorPin2 = 1;
private static final int k_rightMotorPin1 = 2;
private static final int k_rightMotorPin2 = 3;
I2CPWMMotor m_rightMotor = null;
I2CPWMMotor m_leftMotor = null;
Joystick m_joystick = null;
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
Logger.Log("DriveSubsystem", 2, "Init()");
m_leftMotor = new I2CPWMMotor(k_leftMotorPin1, k_leftMotorPin2);
m_rightMotor = new I2CPWMMotor(k_rightMotorPin1, k_rightMotorPin2);
m_joystick = new Joystick(0);
}
public void SetPower(double left, double right)
{
m_leftMotor.set(left);
m_rightMotor.set(right);
}
public void ArcadeDrive()
{
double x = m_joystick.getX();
double y = m_joystick.getY();
x = (x < 0) ? -(x * x) : (x * x);
y = (y < 0) ? -(y * y) : (y * y);
SetPower(y + x, y - x);
}
}
Now we need to call this new ArcadeDrive function during the Teleop mode of our main Robot class. Load the Robot.java file and add the following line to the teleopPeriodic() function:
m_driveSubsystem.ArcadeDrive();
Also, since we no longer need it, remove the SetPower call we previously added to the teleopInit() function.
Your Robot.java file should now look like:
package robot;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
import wpilib.Scheduler;
public class Robot extends IterativeRobot
{
public static final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
public static final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
public static OI m_OI = null;
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotInit()");
m_driveSubsystem.Init();
m_exampleSubsystem.Init();
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();
m_driveSubsystem.ArcadeDrive();
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);
}
}
// END SNIPPET: serial-snippet
If we now run our program, we should be able to control the robot with the joystick.
We are now close to the solution that we need, but we are not quite there yet. We really don’t want to call functions from the DriveSubsystem directly from our main program. The reason is that there may be other actions we want the robot to take which require the use of the drive. If we are always setting the motor power based on the joystick values, then this would interfere with those other operations. To make it work, we would need to disable the joystick operation while those other operations were executing.
Fortunately, the built in Command structure allows this to happen automatically if we use it correctly.
The first thing we need to do is to create an ArcadeDriveCommand which will handle the arcade drive mode.
Right click on the commands folder in the Package Explorer and create a new class called ArcadeDriveCommand:
Once again, we could create this class from scratch, but it will be easier to copy the sample ExampleCommand and use that as a starting point. Open the ExampleCommand.java file and copy all of the text and past it into the ArcadeDriveCommand.java file, replacing the existing text. Then as before do a search and replace changing all instances of ExampleCommand to ArcadeDriveCommand. The resulting file should look like:
package commands;
import robotCore.Logger;
import wpilib.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 let’s make the changed needed to make this work.
In the constructor ArcadeDriveCommand(), we have an opportunity to specify which of the robot’s subsystems, if any, that this command requires. For this command, we will require the DriveSubsystem. Therefore change the requires call to:
requires(Robot.m_driveSubsystem);
Note that specifying all of the required subsystems is very important. This is how the system will tell who needs what subsystems. When one command is running that uses a particular subsystem, and a second command is started which also requires that subsystem, then the first command will be interrupted while the second command runs. This ensures that two commands don’t ‘fight’ over the same subsystem.
We don’t need to do anything special for the initialize() function, so we will leave that alone.
The execute() function is where the action happens. While this command is running, this function will be called repeatedly. This is where we want to add our code to control the drive motors from the joystick. We do this by adding the following line to the execute() function.
Robot.m_driveSubsystem.ArcadeDrive();
Next we need to modify the isFinished() function. This function should return true when the command has finished whatever it is doing, and false if it still has more stuff to do. In this case, the command will never end (unless it is interrupted), so we always want to return false.
Finally, the interrupted() function will be called if this command is interrupted (i.e. another command which requires the drive subsystem is about to run). When this happens, we probably want to turn the motors off so that they aren’t simply left at their last values set by the joystick. Add the following line to the interrupted() function:
Robot.m_driveSubsystem.SetPower(0, 0);
At this point, your ArcadeDriveCommand.java file should look like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class ArcadeDriveCommand extends Command
{
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()");
Robot.m_driveSubsystem.ArcadeDrive();
}
// 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()");
Robot.m_driveSubsystem.SetPower(0, 0);
}
}
Next, we want to set our DriveSubsystem to run this command whenever it has nothing else to do. Open the DriveSubsystem.java file, and add the following line to the initDefaultCommand() function. Note that you will also need to add the import for the ArcadeDriveCommand class.
Your DriveSubsystem.java file should now look like:
package subsystem;
import commands.ArcadeDriveCommand;
import robotCore.I2CPWMMotor;
import robotCore.Joystick;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_leftMotorPin1 = 0;
private static final int k_leftMotorPin2 = 1;
private static final int k_rightMotorPin1 = 2;
private static final int k_rightMotorPin2 = 3;
I2CPWMMotor m_rightMotor = null;
I2CPWMMotor m_leftMotor = null;
Joystick m_joystick = null;
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
setDefaultCommand(new ArcadeDriveCommand());
}
public void Init()
{
Logger.Log("DriveSubsystem", 2, "Init()");
m_leftMotor = new I2CPWMMotor(k_leftMotorPin1, k_leftMotorPin2);
m_rightMotor = new I2CPWMMotor(k_rightMotorPin1, k_rightMotorPin2);
m_joystick = new Joystick(0);
}
public void SetPower(double left, double right)
{
m_leftMotor.set(left);
m_rightMotor.set(right);
}
public void ArcadeDrive()
{
double x = m_joystick.getX();
double y = m_joystick.getY();
x = (x < 0) ? -(x * x) : (x * x);
y = (y < 0) ? -(y * y) : (y * y);
SetPower(y + x, y - x);
}
}
Finally we need to remove the direct call to m_driveSubsystem.ArcadeDrive() that we added to the teleopPeriodic() command of our Robot class in the last chapter.
The Robot.java file should now look like:
package robot;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.DriveSubsystem;
import subsystem.ExampleSubsystem;
import wpilib.Scheduler;
public class Robot extends IterativeRobot
{
public static final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
public static final ExampleSubsystem m_exampleSubsystem = new ExampleSubsystem();
public static OI m_OI = null;
Robot()
{
Logger.Log("Robot", 2, "Robot()");
}
/**
* Called once to initialize the robot
*/
@Override
public void robotInit()
{
Logger.Log("Robot", 2, "robotInit()");
m_driveSubsystem.Init();
m_exampleSubsystem.Init();
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);
}
}
// END SNIPPET: serial-snippet
Now run deploy and run your program and you should find that you can still control it with the joystick. Only now, the DriveSubsystem can be used by other commands without interference from the joystick.