Drive Subsystem
The first subsystem that we are going to create will be one to control the drive motors for the robot. First we need to create a new class which we will call DriveSubsystem. In the Package Explorer within Eclipse, right click on the subsystem folder and choose the New/Class option. Then enter the name DriveSubsystem in the Name field. The rest of the New Java Class dialog should look like:
Then click the Finish button and the new file, DriveSubsystem.java, should appear under the subsystem folder in the Package Explorer on the left. Double click on that file to load it into the editor. Now Subsytem classes need to have a specific format, and the easiest way to start is to simply copy the code from the ExampleSubsystem.java file. Double click on that file, copy all of the code, and paste it into your DriveSubsystem.java file, replacing all of the text. Your DriveSubsystem.java file should now look like:
If we look closely, we will see a number of little red error markers 
. These indicate errors in our program. There are several problems with this file as it stands. The first is that Java requires that the name of the class being defined in a file match the name of the file. The second is that the class ExampleSubsystem is already defined in another file. Of course we don’t want to call this new class ExampleSubsystem in any case, so we are going to do a search and replace and replace the name ExampleSubsystem with DriveSubsystem everywhere in the file. The code we end up with should now look like this:
package subsystem;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
}
public void Init()
{
Logger.Log("DriveSubsystem", 2, "Init()");
}
}
Now all subsystem classes extend the built in Subsystem class, and have at least two function as shown. The first function initDefaultCommand() allows you to specify a command that is to be run whenever this subsystem is not being used elsewhere. Eventually we will want to create a command that will handle driving the robot with the joystick, but for now, we will not set any default commands.
We will use the second function Init to initialize any variables needed by this class. We don’t do this in the constructor because the robot might not be in a state where some of the objects we need to create can be instantiated when this object is created. Instead, we will call this function in the robotInit() function of our main Robot class.
The first thing we need to do is to create variables that will control the left and right motors. For this robot each side of the drive train is controlled by two motors, and each of the motors is controlled by a motor controller (for a total of four controllers). However, the way the robot is wired, we can control both the left motors via a single PWM signal. The same is true for the two right motors. Hence we will need two motor control objects, one for the left two motors and one for the right two motors.
For this robot, the motors are connected to a PWM controller which is connected via an I2C interface. The proper class for controlling these motors is the I2CPWMMotor class. Add the following two lines at the top of our DriveSubsystem class:
I2CPWMMotor m_rightMotor = null; I2CPWMMotor m_leftMotor = null;
After adding these lines, we can once again see that they are marked as errors. If we hover over one of the error markers, we will see that the error is ‘I2CPWMMotor cannot be resolved to a type’. In this case this means that we are not importing the required files for this class. One of the nice features of Eclipse is that if you click on the error marker, many times it will give you suggested fixes. For example if we click on one of the error markers in this case, we will see:
We can see that Eclipse is suggesting that we add an import for the class in question. If we double click on that line, the following line will automatically be added to the top of the file, and the errors will disappear.
import robotCore.I2CPWMMotor;
Now these two lines declare the variables we will use to control the left and right motors, but they have yet to be initialized. We do not initialize them in the constructor because this object is constructed before the underlying robot code has been initialized. Instead we want to initialize these two variables in the Init() function by adding the following two lines to it.
m_leftMotor = new I2CPWMMotor(k_leftMotorPin1, k_leftMotorPin2);
m_rightMotor = new I2CPWMMotor(k_rightMotorPin1, k_rightMotorPin2);
Each of the motors requires two PWM pins for control, and these pin numbers are specified in the constructor for the I2CPWMMotor class. The left motor is controlled by pins 0 and 1, and the right motor is controlled by pins 2 and 3. Now rather than using these numbers directly in our call to the constructor, we are going to define named constants with these values at the top of the class:
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;
The reason we do this is that it keeps all of our important numbers together in on place and not scattered throughout our code. Although it does involve more typing, it is a very good habit to get into.
Next we need to create a function that will allow us to control these motors. Add the following function to the DriveSubsystem class:
public void SetPower(double left, double right)
{
m_leftMotor.set(left);
m_rightMotor.set(right);
}
This function will allow us to set the power for the left and right motors. The values of left and right will range for -1 to +1 and with -1 representing full power the reverse direction and +1 representing full power in the forward direction.
Your DriveSubsystem.java file should now look like:
package subsystem;
import robotCore.I2CPWMMotor;
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;
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);
}
public void SetPower(double left, double right)
{
m_leftMotor.set(left);
m_rightMotor.set(right);
}
}
Now that we have the DriveSubsystem class defined, we need to add it to our main Robot class. So load the Robot.java file and add the following declaration at the beginning of the Robot class declaration:
public static final DriveSubsystem m_driveSubsystem = new DriveSubsystem();
Once again, we will see the line marked with an error marker. Clicking on the marker, it will give us the option to import the DriveSubsystem file. Double click on that option and the following line will be added near the top of the file:
import subsystem.DriveSubsystem;
Now that we have declared an instance of our DriveSubsystem class, we must initialize it in our robotInit() function by adding the following line to that function:
m_driveSubsystem.Init();
Finally we want to use this new DriveSubsystem class to actually turn the robot motors and and make it do something. Add the following line to the teleopInit() function:
m_driveSubsystem.SetPower(0.5, 0.5);
So if we are in Teleop mode and we enable the robot, our teleopInit() function will set both the left and right motors to drive forward at half power. Note, however, that once we turn the motors on, there is no code to turn them back off and they will stay on until we disable the robot.
At this point, your Robot.java file should look like this:
package robot;
import robotCore.IterativeRobot;
import robotCore.Logger;
import subsystem.ExampleSubsystem;
import wpilib.Scheduler;
public class Robot extends IterativeRobot
{
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, "robotiInit()");
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 let’s run our program. Make sure the PuTTY terminal and the remote launcher is still running and use the button on the driver start it again if needed. Then choose the MyMinibot option from the run menu in Eclipse as described before. Once your robot code is running, bring up the driver station, connect to the robot, and then click the Enable button and your robot should drive forward at half speed. Remember that there is currently no code to turn the motors off, so you should be ready to click the Disable button when you want it to stop.