Drive Subsystem

The next thing we want to do is to get our robot driving. To do this we will create a Subsystem. Subsystems are designed to control some aspect of the robot, in this case the drive motors. We are going to need to be able to control the motors using a number of different processes and the advantage of using a Subsystem is that it will enable us to control which process has access to the drive motors at what times.

Rather than create a new subsystem from scratch, we are going to copy the existing ExampleSubsystem. Open the subsystem folder in the panel on the left. Right click on the ExampleSubsystem.java entry and choose Copy:

CopyExampleSubsystem

Now right click on the subsytem folder and click Paste:

PasteExampleSubsystem

 

Then enter DriverSubsystem as the name for the new subsystem class:

DriveSubsystemName

This should add a new file DriveSubsystem.java to your project under the subsystem folder.

Now open the DriveSubsystem.java file by double clicking on it. Copying the class in this manner automatically renames the class name for us, but we also want to rename the strings in all the Logger calls to match. Press Ctl-F to bring up the search and replace box and replace all instances of ExampleSubsystem with DriveSubsystem. After you have done this, your DriveSubsystem.java file should look like:

package subsystem;

import robotCore.Logger;
import robotWpi.command.Subsystem;

/**
 *
 */
public class DriveSubsystem extends Subsystem 
{
    public void initDefaultCommand() 
    {
    	Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
    }
}

Let’s take a look at what we have. The first function initDefaultCommand will allow us to specify a command to run when no other process requires the subsystem. For now, we are not using commands, so we will not do anything here.

So what resources are we going to need? We are going to want to control the left and right motors so we will need some variable to do that. The yellow motors attached to our robot require the PWMMotor class for control. The first thing we must do, then, is to create the variable that will be used to control these motors. Add the following two lines to the top of your DriveSubsystem class:

	private PWMMotor		m_leftMotor;
	private PWMMotor		m_rightMotor;

When we do this Eclipse will show those lines as having errors. If we hover the mouse over the little error marker for the line, it will show us the error:

RobotJavaError1

It is telling us PWMMotor cannot be resolved to a type’. This is because we need to import the package which defines this class. Fortunately, Eclipse has a nice little feature where it can suggest solutions to errors. If we click on the litter error marker we will see options on how to fix it:

RobotJavaFixError1

Here we are presented with a number of options, but we will select the first by double clicking on the ‘Import ‘PWMMotor’ (robotCore)’ option. This should fix the errors, however it will still be showing warning markers. Hovering over the warning marker we will find that it is complaining that the variable is not being used. We will fix that in a moment.

We have declared the variables to control the motors, but we still need to initialize them. We need to be careful when we create the instances of these controllers. We must make sure that they are not instantiated until the robot code has been initialized. However, if we wait to create an instance of this class until the robot is initialized, then it will be safe to initialize these variable in this class’s constructor.

Looking at the documentation for PWMMotor we see the constructor requires 2 integers, a pwmPin and a dirPin. The first pin will control the power of the motor and the second will control the direction. We use a Arduino to provide these signals. For an Arduino, only some of the pins can be configured for PWM (Pulse Width Modulation) output. We will be using the PWM pin 5 for the right motor and PWM pin 6 for the left motor. To control the direction, we will be using pin 4 for the right motor and pin 7 for the left. Given this, we can initialize our motor variables as follows:

public class DriveSubsystem extends Subsystem 
{
	private PWMMotor		m_leftMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
	private PWMMotor		m_rightMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);

Note that rather than explicitly using the pin numbers in the constructors we are using constants which we will define by adding the following lines to the top of the DriveSubsystem class.

	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;

Although doing it this way involves more typing, it is a good habit to get into. It allows us to group together the important constants that control our robot in one place where they will be easy to find and change if needed.

Now that we have defined and initialized the variables that control the motors we need to provide a function that will allow users of this class the ability to set the power on the motors. We will create a function called SetPower which will allow us to set the power of the left and right motors. Once again consulting the documentation for PWMMotor we find that we can set the power on an individual motor by calling it’s set(double power) function. This function takes a single parameter which specifies the power and can range from -1.0 for full reverse to +1.0 for full forward. Given this we will define our SetPower function as follows.

    public void SetPower(double leftPower, double rightPower)
    {
    	m_rightMotor.set(rightPower);
    	m_leftMotor.set(leftPower);
    }

Your DriveSubsystem.java file should now look like:

package subsystem;

import robotWpi.command.Subsystem;
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()");
    }
	
    public void SetPower(double leftPower, double rightPower)
    {
    	m_rightMotor.set(rightPower);
    	m_leftMotor.set(leftPower);
    }	
}

Let’s now add code to the teleopInit() function of our Robot class to use the DriveSubsystem to drive the robot forward at half power.

The first thing we must do is to declare a variable that will let us access the DriveSubsystem by adding the following line at the top of our Robot class. Remember when you do this to import the DriveSubstem class.

	public static DriveSubsystem m_driveSubsystem;

This declares variable m_driveSubsystem which we will use to control the robot. We have made this variable public and static so that we can access it from other modules without needing an instance of our Robot class. Note also that we do NOT initialize this variable with an instance of this class at this point, as we need to wait until the robot code is initialized. When the robot code has been initialized, the robotInit() function will be called, and this is where we should create our instance of DriveSubsystem.

    public void robotInit() 
    {
		Logger.Log("Robot", 2, "robotiInit()");
		
		m_driveSubsystem = new DriveSubsystem();
		m_exampleSubsystem = new ExampleSubsystem();
		m_OI = new OI();
    }

Finally in our teleopInit() function we want to turn the motors on at half power. We can do this by adding the following line to that function:

		m_driveSubsystem.SetPower(0.5,  0.5);

Your Robot.java file should now 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()");
		
		m_driveSubsystem.SetPower(0.5,  0.5);		
	}
	
	/**
     * 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

Before we run our program we need to think a little bit about what will happen. Right now we will turn the motors on when you use the Driver Station to enable the robot when it is in the Teleop mode. Note, however, that nowhere do we turn the motors off. This means that as long as the robot is enabled, it will continue to drive. Hence when we run this program we must be ready to disable the robot in order to make it stop.

Now deploy and run your program and see if it works.

Having the robot run forever is really not that useful, we would really rather have it stop at some point. We will now change our program so that the robot will drive for 2 seconds and then stop. To do this, we will need a timer. Declare and initialize a Timer variable at the top of your Robot class. You will need to import the Timer class. Make sure that it is the Timer class from robotCore not one of the Java timer classes.

	private Timer m_timer = new Timer();

Now in our teleopInit() functions we are turning the motors on, but we also need to reset the timer by adding the following line:

		m_timer.reset();

Finally, in our teleopPeriodic() function we need to check if the elapsed time exceeds 2 seconds and, if so, turn the motors off:

    public void teleopPeriodic()
	{
		Logger.Log("Robot", -1, "teleopPeriodic()");
		
		if (m_timer.get() >= 2)
		{
			m_driveSubsystem.SetPower(0, 0);
		}
		
		Scheduler.getInstance().run();
		
		Sleep(10);
    }

Your Robot.java file should now look like:

package robot;

import robotCore.Timer;
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;
	private Timer m_timer = new Timer();
	
	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()");
		
		m_timer.reset();
		
		m_driveSubsystem.SetPower(0.5,  0.5);		
	}
	
	/**
     * Called periodically during operator control
     */
	@Override
    public void teleopPeriodic()
	{
		Logger.Log("Robot", -1, "teleopPeriodic()");
		
		if (m_timer.get() >= 2)
		{
			m_driveSubsystem.SetPower(0, 0);
		}
		
		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. When you enable the robot from the Driver Station it should now only drive for 2 seconds and then stop automatically.

Next: Commands