Speed Control

Now that we know how to use the encoders, we want to change our DriveSubsystem class so that we can set the motors to run at a specific speed and automatically adjust the power to maintain that speed.

Before we tackle that, however, we need to gather some information as to the relationship between the power applied to the motors and their speed. We are going to create a new command where we will run the robot forward at different power settings and record the speed that results.

Start by adding a new class TestMotorSpeedCommand to the commands folder, and copy the contents of ExampleCommand.java replacing the appropriate strings.

This command will be using the DriveSubsystem, so, as before, we require it in our constructor.

    public TestMotorSpeedCommand() 
    {
    	Logger.Log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");
    	
        // Use requires() here to declare subsystem dependencies
        requires(Robot.m_driveSubsystem);
    }

Now what we want to do is to start the robot at 0 power and gradually increase the power, recording the speed, until we reach full power. To accomplish this we are going to need a member variable to store the current power:

	double	m_power;

When the command starts, we want to set this power to zero:

    protected void initialize() 
    {
    	Logger.Log("TestMotorSpeedCommand", 2, "initialize()");
    	
    	m_power	= 0;
    }

Then in the execute() function we will set the drive power to the current power and then increase the power by 0.01. We also want to slow down how often this function is called so we don’t get too much output and we do this by adding another 40 ms delay.

    protected void execute() 
    {
    	Logger.Log("TestMotorSpeedCommand", -1, "execute()");
    	
    	Robot.m_driveSubsystem.SetPower(m_power, m_power);
    	
    	m_power	+= 0.01;
    	
    	Robot.Sleep(40);
    }

Then we need to change the isFinished() function to return true when the power exceeds 1.3. Note that we are increasing the power up to 1.3 even though the highest power the motor will accept is 1.0 (anything over that will be treated the same as 1.0). The reason we are doing this it see exactly where the speed tops out at.

    protected boolean isFinished() 
    {
    	Logger.Log("TestMotorSpeedCommand", -1, "isFinished()");
        
    	return (m_power >= 1.3);
    }

Finally when the command is finished, we need to turn the motors off.

    protected void end() 
    {
    	Logger.Log("TestMotorSpeedCommand", 2, "end()");
    	
    	Robot.m_driveSubsystem.SetPower(0,  0);
    }

So far, so good, we now have a robot that will accelerate slowly until it reaches is maximum velocity. however we also want to log the data so we can analyze it later. We want the log to be in a CSV (comma separated value) format so that we can easily load it into our spreadsheet for analysis. In the initialize() function, we want to log the spreadsheet titles by adding the lines:

    	Logger.Log("TestMotorSpeedCommand", 3, "...,Time,Power,Left Speed,Right Speed");
    	Logger.ResetElapsedTime();

The second line resets the logger clock to zero.

Next, we need to log the power and speeds each time the execute() function is called. We do this by adding the following code.

    protected void execute() 
    {
    	Logger.Log("TestMotorSpeedCommand", -1, "execute()");
    	
        Robot.m_driveSubsystem.SetPower(m_power, m_power);
        
        m_power    += 0.01;
        
        Robot.Sleep(40);
        
        Logger.Log("TestMotorSpeedCommand", 3, String.format(",%d,%.2f,%d,%d", 
                Logger.GetElapsedTime(), 
                m_power,
                Robot.m_driveSubsystem.GetLeftEncoder().getSpeed(), 
                Robot.m_driveSubsystem.GetRightEncoder().getSpeed()));        
    }

This logs the time, power and left and right motor speeds.

Your TestMotorSpeedCommand.java file should now look like:

package commands;

import robotCore.Logger;
import robotWpi.command.Command;
import robot.Robot;

/**
 *
 */
public class TestMotorSpeedCommand extends Command 
{
	double    m_power;
	
    public TestMotorSpeedCommand() 
    {
    	Logger.Log("TestMotorSpeedCommand", 3, "TestMotorSpeedCommand()");
    	
        // 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("TestMotorSpeedCommand", 2, "initialize()");
    	
    	Logger.Log("TestMotorSpeedCommand", 3, "...,Time,Power,Left Speed,Right Speed");
    	Logger.ResetElapsedTime();
    	
    	m_power	= 0;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() 
    {
    	Logger.Log("TestMotorSpeedCommand", -1, "execute()");
    	
        Robot.m_driveSubsystem.SetPower(m_power, m_power);
        
        m_power    += 0.01;
        
        Robot.Sleep(40);
        
        Logger.Log("TestMotorSpeedCommand", 3, String.format(",%d,%.2f,%d,%d", 
                Logger.GetElapsedTime(), 
                m_power,
                Robot.m_driveSubsystem.GetLeftEncoder().getSpeed(), 
                Robot.m_driveSubsystem.GetRightEncoder().getSpeed()));        
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() 
    {
    	Logger.Log("TestMotorSpeedCommand", -1, "isFinished()");
        
    	return (m_power >= 1.3);
    }

    // Called once after isFinished returns true
    protected void end() 
    {
    	Logger.Log("TestMotorSpeedCommand", 2, "end()");
    	
    	Robot.m_driveSubsystem.SetPower(0,  0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() 
    {
    	Logger.Log("TestMotorSpeedCommand", 2, "interrupted()");
    	
    	end();
    }
}

Now edit the OI.java file and change the m_testButton command to run this new TestMotorSpeedCommand instead of the DriveForDistanceCommand . Your OI.java file should now look like:

package robot;

import robotCore.Joystick;
import commands.DriveForTimeCommand;
import commands.TestMotorSpeedCommand;
import robotWpi.buttons.Button;
import robotWpi.buttons.JoystickButton;;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
     Joystick m_stick = new Joystick(0);
     Button m_trigger = new JoystickButton(m_stick, 1);
     Button m_testButton = new JoystickButton(m_stick, 2);
     
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    public OI()
    {
    	m_trigger.whenPressed(new DriveForTimeCommand(0.75, 2));
    	m_testButton.whenPressed(new TestMotorSpeedCommand());
    }
}

Now deploy and run your program. After you have connected to the robot and enabled teleop mode, switch back to Eclipse and right click on the Console window and click Clear to clear the window.

RobotJavaClearConsole

Now press button 2 on the joystick to get the command to run. This will log a large amount of data into the Console window of Eclipse:

RobotJavaSpeedLog

Now, select all of this data in the Console window by pressing ctl-a and copy it to the clipboard by pressing ctl-c. We will use LibreOffice to view the data. Create a new Calc Spreadsheet and use the Edit / Paste Special command to past the data from Eclipse as Unformatted text:

RobotJavaPasteCalc

Be sure to choose Comma as the Separator Options:

RobotJavaCommaSep

Now, delete the first two rows, and the first column. Then select the Time, Left Speed, and Right Speed columns:

RobotJavaCalcSelect

Now choose the Insert / Chart menu option. For the Chart Type choose XY (Scatter) and choose the lines Only option:

RobotJavaChartType

This should then result in the following chart:

There are a couple of things we see here. The first is that the left motor is running slightly faster than the right which is why the robot tends to drift to the right (note that the motor speeds may differ on your particular robot). We can also see that the maximum speed of the right motor is somewhere around 1700 units/second. Since we will want our robot to drive straight at all speeds we will use this value as the maximum speed for both motors. Finally we can see that the motors won’t even start turning until the power reaches about 0.25. We can now use this information to create speed based controllers for the motors.

The SmartMotor class has a built in mechanism to control the speed of the motor using a PID controller.  PID stands for Proportional, Integral, Differential.  When we use this control mechanism, we will set a target for the speed we desire (called the setPoint) and the SmartMotor class will adjust the power sent to the motor based on the following formula:

  power = (P * error) + (I * totalError) + (D * deltaError) + (F * setPoint)

where

  • P – represents the proportional scale factor.
  • error – represents the difference between the current speed and the setPoint.
  • I – represents the integral scale factor.
  • totalError – represents the total accumulated error over time.
  • D – represents the derivative scale factor.
  • deltaError – represents the difference between the current error and the last.
  • F – represents the constant scale factor which is proportional to the setPoint.
  • setPoint – represents the current target speed.

The trick is picking the right combination of P, I, D and F to make the motor behave like you want.

We will start with the F term which represents a constant power which proportional to the setPoint.  Now we know that at full power the maximum speed it about 1700 units/sec.  Thus we need a F term that, when multiplied by the max speed (i.e. 1700) will result in full power being applied (i.e. a power of 1.0).  That number is 1/1700 or about 0.00059, which is the number we will use.

Note if we use only the constant term, this will essentially be the same as controlling the motor by power alone.  If there is more or less resistance to the motors, no adjustments will occur and the motors will slow down or speed up.  However, we want to get this term set so that the motors run close to the target speed we desire.

To set the F term, we add the following two lines to the DriveSubsystem constructor:

        m_leftMotor.setFTerm(k_F);
        m_rightMotor.setFTerm(k_F);

And, of course we must define k_F as follows:

	private static final double k_F				= 0.00059;

Also, in order for the motor controller to control the speed of the motors, it will need to know what encoders are being used to measure the speed. We set the by adding the following two lines to the DriveSubsystem constructor:

		m_leftMotor.setFeedbackDevice(m_leftEncoder);
		m_rightMotor.setFeedbackDevice(m_rightEncoder);

The last thing we want to do is to set a scale factor so when we want to set the speed, we can just use a range from -1.0 to +1.0 like we do with other motors.  Otherwise we would need to set the speed based on the arbitrary units of the encoder.  We do this by adding the following two lines to the constructor:

        m_leftMotor.setMaxSpeed(k_maxSpeed);
        m_rightMotor.setMaxSpeed(k_maxSpeed);

Defining k_maxSpeed at the top of the class as follows:

	private static final int k_maxSpeed			= 1700;

Finally, we could change the SetPower function of our DriveSubsystem class to set the speed instead, but there may be a time in the future where we still want to control the power.  Therefore, we will create a new function SetSpeed which we will use to set the speed as follows:

    public void SetSpeed(double leftSpeed, double rightSpeed)
    {
    	m_leftMotor.setControlMode(SmartMotorMode.Speed);
    	m_rightMotor.setControlMode(SmartMotorMode.Speed);
    	
    	m_leftMotor.set(leftSpeed);
    	m_rightMotor.set(rightSpeed);
    }

First we ensure that the controller is in speed mode by calling the setControlMode function. Then we set the desired speeds.

We also need to change the SetPower function to set the control mode back to power by adding the following  to SetPower:

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

You DriveSubsystem.java file should now look like:

package subsystem;

import robotWpi.command.Subsystem;
import commands.ArcadeDriveCommand;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;

/**
 *
 */
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 static final int k_leftEncoderPin1	= 0;
	private static final int k_leftEncoderPin2	= 1;
	private static final int k_rightEncoderPin1	= 2;
	private static final int k_rightEncoderPin2	= 3;
	private static final int k_maxSpeed			= 1700;
	private static final double k_F				= 0.00059;
	
	private PWMMotor		m_leftMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
	private PWMMotor		m_rightMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
	
	private Encoder 	m_rightEncoder	= new Encoder(robotCore.Encoder.EncoderType.Analog, k_rightEncoderPin1, k_rightEncoderPin2);
	private Encoder 	m_leftEncoder	= new Encoder(robotCore.Encoder.EncoderType.Analog, k_leftEncoderPin1, k_leftEncoderPin2);
	
	public DriveSubsystem()
	{
		m_leftEncoder.setInverted(true);
		
		m_leftMotor.setFeedbackDevice(m_leftEncoder);
		m_rightMotor.setFeedbackDevice(m_rightEncoder);
        m_leftMotor.setMaxSpeed(k_maxSpeed);
        m_rightMotor.setMaxSpeed(k_maxSpeed); 
        m_leftMotor.setFTerm(k_F);
        m_rightMotor.setFTerm(k_F);
	}
	
	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_leftMotor.setControlMode(SmartMotorMode.Power);
    	m_rightMotor.setControlMode(SmartMotorMode.Power);
    	    	
    	m_rightMotor.set(rightPower);
    	m_leftMotor.set(leftPower);
    }
    
    public void SetSpeed(double leftSpeed, double rightSpeed)
    {
    	m_leftMotor.setControlMode(SmartMotorMode.Speed);
    	m_rightMotor.setControlMode(SmartMotorMode.Speed);
    	
    	m_leftMotor.set(leftSpeed);
    	m_rightMotor.set(rightSpeed);
    }
    
    public Encoder GetLeftEncoder()
    {
    	return(m_leftEncoder);
    }
    
    public Encoder GetRightEncoder()
    {
    	return(m_rightEncoder);
    }    
}

Next we want to add a new command that we can use to test out our speed control. Create a new class under the commands folder called TestSpeedControlCommand and, as before, copy over the code from ExampleCommand, renaming the appropriate fields.

Now this command will use the DriveSubsystem, so require it in the constructor.

    public TestSpeedControlCommand() 
    {
    	Logger.Log("TestSpeedControlCommand", 3, "TestSpeedControlCommand()");
    	
        // Use requires() here to declare subsystem dependencies
        requires(Robot.m_driveSubsystem);
    }

Now for this test, we are simply going to run the robot at a constant speed and log the results so that we can see how close to that speed we come. Therefore in the initialize() function we will simply use our new SetSpeed(…) function to start the robot forward at half speed. Since we will also be logging the speed, we log the title row for our CSV file.

    protected void initialize() 
    {
    	Logger.Log("TestSpeedControlCommand", 2, "initialize()");
    	
    	Logger.Log("TestSpeedControlCommand", 3, "...,Time,Speed,Left Speed, Right Speed");
    	
    	Robot.m_driveSubsystem.SetSpeed(k_speed,  k_speed);
    }

Of course defining k_speed:

	private static final double k_speed = 0.5;

In the execute() function we want to log the current time, power, and speeds of the two motors. We will also slow down the output by adding another 40 ms delay.

    protected void execute() 
    {
    	Logger.Log("TestSpeedControlCommand", -1, "execute()");
    	
    	Logger.Log("TestSpeedControl", 3, String.format("%d,%.2f,%d,%d", 
    							Logger.GetElapsedTime(),
    							k_speed,
    							Robot.m_driveSubsystem.GetLeftEncoder().getSpeed(),
    							Robot.m_driveSubsystem.GetRightEncoder().getSpeed()));
    	
    	Robot.Sleep(40);
    }

Now for this command we are going to make it run as long as we hold down button 2 on the joystick. Since the command will never end on it’s own, we want to always return false from the isFinished() function.

When we release button 2 on the joystick, this command will be interrupted and it’s interrupted() function will be called. At that point we need to turn the motors back off. Note that for consistency we turn the motors off in the end() function (which will not be called directly in this case), and call end() from our interrupted() function.

    // Called once after isFinished returns true
    protected void end() 
    {
    	Logger.Log("TestSpeedControlCommand", 2, "end()");
    	
    	Robot.m_driveSubsystem.SetSpeed(0,  0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() 
    {
    	Logger.Log("TestSpeedControlCommand", 2, "interrupted()");
    	
    	end();
    }

Your TestSpeedControlCommand.java file should now look like:

package commands;

import robotCore.Logger;
import robotWpi.command.Command;
import robot.Robot;

/**
 *
 */
public class TestSpeedControlCommand extends Command 
{
	private static final double k_speed = 0.5;
	
    public TestSpeedControlCommand() 
    {
    	Logger.Log("TestSpeedControlCommand", 3, "TestSpeedControlCommand()");
    	
        // 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("TestSpeedControlCommand", 2, "initialize()");
    	
    	Logger.Log("TestSpeedControlCommand", 3, "...,Time,Speed,Left Speed, Right Speed");
    	
    	Robot.m_driveSubsystem.SetSpeed(k_speed,  k_speed);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() 
    {
    	Logger.Log("TestSpeedControlCommand", -1, "execute()");
    	
    	Logger.Log("TestSpeedControlCommand", 3, String.format("...,%d,%.2f,%d,%d", 
				Logger.GetElapsedTime(),
				k_speed,
				Robot.m_driveSubsystem.GetLeftEncoder().getSpeed(),
				Robot.m_driveSubsystem.GetRightEncoder().getSpeed()));

    	Robot.Sleep(40);    
}

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() 
    {
    	Logger.Log("TestSpeedControlCommand", -1, "isFinished()");
        
    	return (false);
    }

    // Called once after isFinished returns true
    protected void end() 
    {
    	Logger.Log("TestSpeedControlCommand", 2, "end()");
    	
    	Robot.m_driveSubsystem.SetSpeed(0,  0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() 
    {
    	Logger.Log("TestSpeedControlCommand", 2, "interrupted()");
    	
    	end();
    }
}

Now connect our test button 2 to our new TestSpeedControlCommand by changing the the OI() constructor as follows:

    public OI()
    {
    	m_trigger.whenPressed(new DriveForTimeCommand(0.75, 2));
    	m_testButton.whileHeld(new TestSpeedControlCommand());
    }

Notice that we are using the whileHeld(…) function here instead of the whenPressed(…) function. This will cause our command to be run only while the specified button is being held down. When it is released, our command will automatically be interrupted.

Your OI.java file should now look like:

package robot;

import robotCore.Joystick;
import commands.DriveForTimeCommand;
import commands.TestSpeedControlCommand;
import robotWpi.buttons.Button;
import robotWpi.buttons.JoystickButton;;

/**
 * This class is the glue that binds the controls on the physical operator
 * interface to the commands and command groups that allow control of the robot.
 */
public class OI {
    //// CREATING BUTTONS
    // One type of button is a joystick button which is any button on a joystick.
    // You create one by telling it which joystick it's on and which button
    // number it is.
     Joystick m_stick = new Joystick(0);
     Button m_trigger = new JoystickButton(m_stick, 1);
     Button m_testButton = new JoystickButton(m_stick, 2);
     
    // There are a few additional built in buttons you can use. Additionally,
    // by subclassing Button you can create custom triggers and bind those to
    // commands the same as any other Button.
    
    //// TRIGGERING COMMANDS WITH BUTTONS
    // Once you have a button, it's trivial to bind it to a button in one of
    // three ways:
    
    // Start the command when the button is pressed and let it run the command
    // until it is finished as determined by it's isFinished method.
    public OI()
    {
    	m_trigger.whenPressed(new DriveForTimeCommand(0.75, 2));
    	m_testButton.whileHeld(new TestSpeedControlCommand());
    }
}

Now run your program and hold down button 2 for a few seconds. Be sure to clear the Console window first. After the program runs, select and copy the data from the Console window and past it into a LibreOffice spreadsheet. Plotting the data as we did before should give you a chart something like:

HalfSpeedFTerm1

Since we are running at half speed, we should expect a speed of around 850. Both motors are running a slower than we want so we are going to increase the F parameter to 0.0007. Doing so results in the following graph:

HalfSpeedFTerm2

 

Once again, we see that the right motor runs slower than the left. We could set a different F term for the right, but these values are close enough four our purposes.

Next we will add a P term which adds a correction which is proportional to the difference between the desired speed and the actual speed (i.e. the error). We need to pick a reasonable number for this. Let’s say that we want to increase the power by 25% when the error term is 1000. To do this we would need a P term of 0.00025.

To set the P term, add the following lines to the constructor of your DriveSubsystem class:

        m_leftMotor.setPTerm(k_P);
        m_rightMotor.setPTerm(k_P);

And define k_P as follows:

	private static final double k_P				= 0.00025;

Your DriveSubsystem.java file should now look like:

package subsystem;

import robotWpi.command.Subsystem;
import commands.ArcadeDriveCommand;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;

/**
 *
 */
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 static final int k_leftEncoderPin1	= 0;
	private static final int k_leftEncoderPin2	= 1;
	private static final int k_rightEncoderPin1	= 2;
	private static final int k_rightEncoderPin2	= 3;
	private static final int k_maxSpeed			= 1700;
	private static final double k_F				= 0.0007;
	private static final double k_P				= 0.00025;
	
	private PWMMotor		m_leftMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
	private PWMMotor		m_rightMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
	
	private Encoder 	m_rightEncoder	= new Encoder(robotCore.Encoder.EncoderType.Analog, k_rightEncoderPin1, k_rightEncoderPin2);
	private Encoder 	m_leftEncoder	= new Encoder(robotCore.Encoder.EncoderType.Analog, k_leftEncoderPin1, k_leftEncoderPin2);
	
	public DriveSubsystem()
	{
		m_leftEncoder.setInverted(true);
		
		m_leftMotor.setFeedbackDevice(m_leftEncoder);
		m_rightMotor.setFeedbackDevice(m_rightEncoder);
        m_leftMotor.setMaxSpeed(k_maxSpeed);
        m_rightMotor.setMaxSpeed(k_maxSpeed); 
        m_leftMotor.setFTerm(k_F);
        m_rightMotor.setFTerm(k_F);
        m_leftMotor.setPTerm(k_P);
        m_rightMotor.setPTerm(k_P);        
	}
	
	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_leftMotor.setControlMode(SmartMotorMode.Power);
    	m_rightMotor.setControlMode(SmartMotorMode.Power);
    	    	
    	m_rightMotor.set(rightPower);
    	m_leftMotor.set(leftPower);
    }
    
    public void SetSpeed(double leftSpeed, double rightSpeed)
    {
    	m_leftMotor.setControlMode(SmartMotorMode.Speed);
    	m_rightMotor.setControlMode(SmartMotorMode.Speed);
    	
    	m_leftMotor.set(leftSpeed);
    	m_rightMotor.set(rightSpeed);
    }
    
    public Encoder GetLeftEncoder()
    {
    	return(m_leftEncoder);
    }
    
    public Encoder GetRightEncoder()
    {
    	return(m_rightEncoder);
    }    
}

Now run your program and graph the speed results. You should get something like:

HalfSpeedPTerm1

Now we see both the left and right speeds to be much closer to the target value of 850. You may also notice that your robot drives much straighter now.

Finally we want to add an I term to give the robot the ability to adjust the power automatically if the robot meets more or less resistance than expected.  Remember that the I term is proportional to the accumulated error over time.  This means as long as the speed is less than the setPoint, the accumulated error will grow and the power applied will increase making the motors run faster. Similarly, as long as the speed is greater than the setPoint, the accumulated error will decrease making the power applied decrease, slowing the robot.

Picking the right number for the I term is tricky.  If we make the number too small, it will take a long time to reach the setPoint, if we make the number too large, it will overshoot the setPoint and oscillate.  To illustrate this, lets start by setting the I term to 0.0006. In addition to setting the I value we also need to set a value called the IZone. This specifies the region in which the integral term will be applied. Any speeds outside of this range will only use the F and P terms. We do this to prevent us accumulating an large error term while the motor is starting up.

Both of these values are set by adding the following to the constructor of your DriveSubsystem class:

        m_leftMotor.setITerm(k_I);;
        m_rightMotor.setITerm(k_I);;
        m_leftMotor.setIZone(k_IZone);
        m_rightMotor.setIZone(k_IZone);

Of course declaring the constants as follows:

	private static final double k_I				= 0.0006;
	private static final double k_IZone			= 100;

Your DriveSubsytem.java file should now look like:

package subsystem;

import robotWpi.command.Subsystem;
import commands.ArcadeDriveCommand;
import robotCore.Encoder;
import robotCore.Logger;
import robotCore.PWMMotor;
import robotCore.SmartMotor.SmartMotorMode;

/**
 *
 */
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 static final int k_leftEncoderPin1	= 0;
	private static final int k_leftEncoderPin2	= 1;
	private static final int k_rightEncoderPin1	= 2;
	private static final int k_rightEncoderPin2	= 3;
	private static final int k_maxSpeed			= 1700;
	private static final double k_F				= 0.0007;
	private static final double k_P				= 0.00025;
	private static final double k_I				= 0.0006;
	private static final double k_IZone			= 100;
	
	private PWMMotor		m_leftMotor = new PWMMotor(k_rightMotorPWMPin, k_rightMotorDirPin);
	private PWMMotor		m_rightMotor = new PWMMotor(k_leftMotorPWMPin, k_leftMotorDirPin);
	
	private Encoder 	m_rightEncoder	= new Encoder(robotCore.Encoder.EncoderType.Analog, k_rightEncoderPin1, k_rightEncoderPin2);
	private Encoder 	m_leftEncoder	= new Encoder(robotCore.Encoder.EncoderType.Analog, k_leftEncoderPin1, k_leftEncoderPin2);
	
	public DriveSubsystem()
	{
		m_leftEncoder.setInverted(true);
		
		m_leftMotor.setFeedbackDevice(m_leftEncoder);
		m_rightMotor.setFeedbackDevice(m_rightEncoder);
        m_leftMotor.setMaxSpeed(k_maxSpeed);
        m_rightMotor.setMaxSpeed(k_maxSpeed); 
        m_leftMotor.setFTerm(k_F);
        m_rightMotor.setFTerm(k_F);
        m_leftMotor.setPTerm(k_P);
        m_rightMotor.setPTerm(k_P);  
        m_leftMotor.setITerm(k_I);;
        m_rightMotor.setITerm(k_I);;
        m_leftMotor.setIZone(k_IZone);
        m_rightMotor.setIZone(k_IZone);
	}
	
	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_leftMotor.setControlMode(SmartMotorMode.Power);
    	m_rightMotor.setControlMode(SmartMotorMode.Power);
    	    	
    	m_rightMotor.set(rightPower);
    	m_leftMotor.set(leftPower);
    }
    
    public void SetSpeed(double leftSpeed, double rightSpeed)
    {
    	m_leftMotor.setControlMode(SmartMotorMode.Speed);
    	m_rightMotor.setControlMode(SmartMotorMode.Speed);
    	
    	m_leftMotor.set(leftSpeed);
    	m_rightMotor.set(rightSpeed);
    }
    
    public Encoder GetLeftEncoder()
    {
    	return(m_leftEncoder);
    }
    
    public Encoder GetRightEncoder()
    {
    	return(m_rightEncoder);
    }    
}

Now run your program. You will find that the robot speed oscillates as it drives. If you plot the results, you will see something like:

HalfSpeedITerm1

The reason we are seeing the oscillation is because we have made the k_I term too large. Let’s change the k_I term to 0.0008 and see how that works:

	private static final double k_I				= 0.0008;

Run your program and pull and graph the log file and you should see something like:

HalfSpeedITerm2

Much better. Before we wrap up, let’s see how it behaves at a couple of other speeds.  Change the SetSpeed function and make a graph for 0.75 and 0.25 speeds.  You should see something like:

SpeedControl75

SpeedControl25

You can try and further tweak these, and even use different parameters for the left and right motors if you like. One caution, however.  If you set the IRange too low and the motor never gets to a speed that is within that range, then your I term will never kick in and the motor will never reach the desired speed.

Let’s make one last change.  Change the call to SetPower in the ArcadeDrive() function to SetSpeed.  This will allow us to control the robot by speed even during teleop. 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();
    	double	x	= m_joystick.getX();
    	
    	x	= x * x * x;
    	y	= y * y * y;
    	
    	Robot.m_driveSubsystem.SetSpeed(y + x, y - x);
    }

    // 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()");
    }
}

Next: Drive Straight