Speed Control
Well we now have the robot driving, shooting and capturing the ball from the ground so we are done, right? Well, not quite. In fact, now comes the hard part. We want the robot to do much more than just handling the basic teleop controls. In particular we would like the robot to be able to line up a shot on it’s own, to help the driver. In fact, it would be really cool if we could place the robot anywhere on the field, press a button, and have it drive over to the tower and make the shot completely autonomously.
It turns out that such things are possible, but it will take a bit of work to get there.
The first thing we need to do is to change things so that we can set a desired speed for the various motors rather than just a power. The trouble with only controlling the power of the motor is that the resulting speed at which the motor runs is affected by a number of things that are out of our control, such as the current battery level and how much resistance the motor is overcoming at the moment.
When you are driving the robot in teleop mode, this is less of an issue. As a person driving the robot, you can see that you are not applying enough power to make the robot move or turn as you want, and can simply move the joystick to compensate. When the robot is running autonomously, however, this does not happen. For example if the robot, for whatever reason, experiences more resistance that was expected, then it can slow down, or even stall. If the spinning wheels are not spinning at the correct speed, it can affect the accuracy of the shot.
What we need is a way to set the speed that we want a motor to run and have the robot automatically adjust the power to keep that motor running at the specified speed.
Fortunately there is a class called PIDController which can do exactly that. The PID in the name stands for Proportional, Integral, Differential. When we use this controller, we will set a target for the speed we desire (called the setPoint) and the PIDController 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.
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.
Before diving it and programming our PIDController we need to gather some information about the relationship between the power applied to the motor and it’s speed. Specifically we will want to know how fast the motor will run when operating under full power. Now we could just print out the speed and run the motors at full power and look at the result, but I want to set up a system that will let us be more systematic, especially when it comes to the point where we are trying to fine tune the P, I, D and F parameters.
To that end, let’s create a special command which we will tie to one of the buttons that will let us run various tests and log the results. Let’s call this command DriveSpeedTestCommand. You should have created enough new commands by now that you can add this one on your own. Set up the framework for this command and change the OI class so that it will be called when you press button 12. Also remember that since this command will be using the drive motors, it will require the DriveSubsystem.
We are going to pause this tape now while you complete this task. Once you are done, you can check the solution below to make sure you have it right (please no cheating!).
.
.
.
.
.
Now that you have the new command implemented, the DriveSpeedTestCommand.java command should look something like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class DriveSpeedTestCommand extends Command
{
public DriveSpeedTestCommand()
{
Logger.Log("DriveSpeedTestCommand", 3, "DriveSpeedTestCommand()");
// 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("DriveSpeedTestCommand", 2, "initialize()");
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveSpeedTestCommand", -1, "execute()");
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveSpeedTestCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveSpeedTestCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveSpeedTestCommand", 2, "interrupted()");
}
}
and your OI.java file should look like:
package robot;
import commands.DriveSpeedTestCommand;
import commands.IntakeCommand;
import commands.ShootCommand;
import commands.SpinupCommand;
import robotCore.Joystick;
import wpilib.Button;
import wpilib.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_spinup = new JoystickButton(m_stick, 2);
Button m_intake = new JoystickButton(m_stick, 8);
Button m_test = new JoystickButton(m_stick, 12);
// 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 ExampleCommand());
m_spinup.whileHeld(new SpinupCommand());
m_trigger.whileHeld(new ShootCommand());
m_intake.whileHeld(new IntakeCommand());
m_test.whileHeld(new DriveSpeedTestCommand());
}
}
We now want to add code that will drive the robot forward at accelerating from zero to full power while recording the speed of the motors.
If we are going to want to measure the speed of the motors, we are going to need some kind of encoder that will give us that information. The MiniBot has two such encoders attached to the left and right drive trains. Each of these encoders use two potentiometers which can be connected to the analog inputs of the Arduino. By reading the voltage from the potentiometers, we can determine both the position and speed of the wheels.
The class which handles this particular type of encoder is called AnalogEncoder. We need to create two instances of this class, one for the left wheels, and one for the right. Rather than declaring these variables in our newly created DriveSpeedTestCommand, we are going to add them to the DriveSubsystem because that is where they will be used in practice.
Add the following two lines to the top of your DriveSubsystem class:
private AnalogEncoder m_rightEncoder = null; private AnalogEncoder m_leftEncoder = null;
Then initialize them by adding the following to the Init() function of your DriveSubsystem class:
m_rightEncoder = new AnalogEncoder(k_rightEncoderNo, true);
m_leftEncoder = new AnalogEncoder(k_leftEncoderNo, true);
As before, we are using defined constants for the encoder number required by the constructor, so we need to define these at the top of the class. Looking at the AnalogEncoder documentation we see that the left encoder which is attached to analog pins 2 and 3 should be encoder 1, while the right encoder which is attached to pins 0 and 1 should be encoder 0.
private static final int k_leftEncoderNo = 1; private static final int k_rightEncoderNo = 0;
Now in our DriveSpeedTestCommand we are going to want to access these encoders. Rather than making the variables public and accessing them directly, we are going to create two functions which will give us access as follows:
public AnalogEncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public AnalogEncoder GetRightEncoder()
{
return(m_rightEncoder);
}
Your DriveSubsystem.java file should now look like:
package subsystem;
import commands.ArcadeDriveCommand;
import robotCore.AnalogEncoder;
import robotCore.I2CPWMMotor;
import robotCore.Joystick;
import robotCore.Logger;
import wpilib.Subsystem;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_leftEncoderNo = 1;
private static final int k_rightEncoderNo = 0;
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;
private I2CPWMMotor m_rightMotor = null;
private I2CPWMMotor m_leftMotor = null;
private Joystick m_joystick = null;
private AnalogEncoder m_rightEncoder = null;
private AnalogEncoder m_leftEncoder = null;
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
setDefaultCommand(new ArcadeDriveCommand());
}
public void Init()
{
m_leftMotor = new I2CPWMMotor(k_leftMotorPin1, k_leftMotorPin2);
m_rightMotor = new I2CPWMMotor(k_rightMotorPin1, k_rightMotorPin2);
m_joystick = new Joystick(0);
m_rightEncoder = new AnalogEncoder(k_rightEncoderNo, true);
m_leftEncoder = new AnalogEncoder(k_leftEncoderNo, true);
Logger.Log("DriveSubsystem", 2, "Init()");
}
public void SetPower(double left, double right)
{
Logger.Log("DriveSubsystem", -1, String.format("SetPower: l=%.2f, r=%.2f", left, right));
m_leftMotor.set(left);
m_rightMotor.set(right);
}
public void ArcadeDrive()
{
double x = m_joystick.getX();
double y = m_joystick.getY();
Logger.Log("DriveSubsystem", -1, String.format("ArcadeDrive: x=%.2f, y=%.2f", x, y));
x = (x < 0) ? -(x * x) : (x * x);
y = (y < 0) ? -(y * y) : (y * y);
SetPower(y + x, y - x);
}
public AnalogEncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public AnalogEncoder GetRightEncoder()
{
return(m_rightEncoder);
}
}
Now let’s switch back to our DriveSpeedTestCommand class. When this command is executed, what we want to do is ramp up the power starting at zero and record the speed of the motors as a function of the power. In the Initialize() function, we need to reset the encoders, so they start at zero. We can do this by adding the following lines.
Robot.m_driveSubsystem.GetLeftEncoder().reset();
Robot.m_driveSubsystem.GetRightEncoder().reset();
We are also going to want to log the left and right encoder speeds to a file so that we can look at them later. We can do this with the Logger class by also adding the following to the initialize() function.
Logger.SetLogFile("DriveSpeedTest", "DriveSpeedTest.csv");
Logger.Log("DriveSpeedTest", 3, "Time,Left,Right,Power");
Logger.ResetElapsedTime();
The first line creates a log file named DriveSpeedTest.csv associated with the tag DriveSpeedTest, The second line writes a header line into the file, and the last line resets the logger elapsed timer to zero.
We are also going to need a variable to store the current power level. Add the following line to just before the initialize() function:
double m_currentPower;
And initialize it in the initialize() function with the following line:
m_currentPower = 0;
In the execute() function we want to ramp up the power and then write out the current encoder values each time it is called by adding the following lines:
Logger.Log("DriveSpeedTest", 3, String.format("%d,%.2f,%d,%d",
Logger.GetElapsedTime(),
m_currentPower,
Robot.m_driveSubsystem.GetLeftEncoder().getRate(),
Robot.m_driveSubsystem.GetRightEncoder().getRate()));
Robot.m_driveSubsystem.SetPower(m_currentPower, m_currentPower);
m_currentPower += 0.01;
Robot.Sleep(40);
Finally we need to turn the motors off in the interrupted() function:
Robot.m_driveSubsystem.SetPower(0, 0);
Here we are logging the current time, left and right positions, and the current power. We are also setting the drive power to the current power and then incrementing the power by 0.01.
Note that we have also added a Sleep call which adds another 40 ms to the command loop (which also has a 10 ms delay in the teleopPeriodic() function. Doing so will make it so that it will only print out about 20 lines per second, which is plenty fast enough.
At this point, your DriveSpeedTestCommand.java file should look like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class DriveSpeedTestCommand extends Command
{
public DriveSpeedTestCommand()
{
Logger.Log("DriveSpeedTestCommand", 3, "DriveSpeedTestCommand()");
// Use requires() here to declare subsystem dependencies
requires(Robot.m_driveSubsystem);
}
double m_currentPower;
// Called just before this Command runs the first time
protected void initialize()
{
Logger.Log("DriveSpeedTestCommand", 2, "initialize()");
Robot.m_driveSubsystem.GetLeftEncoder().reset();
Robot.m_driveSubsystem.GetRightEncoder().reset();
Logger.SetLogFile("DriveSpeedTest", "DriveSpeedTest.csv");
Logger.Log("DriveSpeedTest", 3, "Time,Power,Left,Right");
Logger.ResetElapsedTime();
m_currentPower = 0;
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveSpeedTestCommand", -1, "execute()");
Logger.Log("DriveSpeedTest", 3, String.format("%d,%.2f,%d,%d",
Logger.GetElapsedTime(),
m_currentPower,
Robot.m_driveSubsystem.GetLeftEncoder().getRate(),
Robot.m_driveSubsystem.GetRightEncoder().getRate()));
Robot.m_driveSubsystem.SetPower(m_currentPower, m_currentPower);
m_currentPower += 0.01;
Robot.Sleep(40);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveSpeedTestCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveSpeedTestCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveSpeedTestCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetPower(0, 0);
}
}
Now deploy and run your program. When you hold down button 12, the robot should drive forward. Let it drive until it reaches full speed.
After you have run the program, you can use an FTP program (such as FileZilla) to connect to the robot and retrieve the file. By default the file will placed in the folder /home/pi/logs and will be named DriveSpeedTest followed by a date/timestamp. Copy this file to your computer, and then load it as a CSV file into Excel. If we then graph the data, we will see the following:
There are a couple of things we can see from this graph. The first is that the motors don’t even start until the power reaches about 0.2.
The second thing we see is that the maximum speed for the motors is about 500 units/second. Note that the units are somewhat arbitrary and depend on the exact nature of the encoder and how the wheels are geared. We could probably find a way to convert this into, for example, RPM, but the arbitrary units will suit us just fine.
Thirdly, once the motors are running, the power vs speed curve is nearly linear, which simplifies things.
Finally we see is the the left and right motors run at about the same speed at any given power. This is a good thing, since it means that the two sides are fairly well balanced.
xxx
We are now ready to use a PIDController to control the speed of the drive motors. However, rather than using the PIDController directly, we are going to use the class MotorSpeedController which will bring together an normal motor controller such as our I2PWMMotor, a speed controller such as our AnalogEncoder, and an PIDController into a class which will behave much like a power based motor controller, except it will allow us to control the speed instead of the power.
The first step is to open your DriveSubsystem.java file and add two new variable to manage the speed of the left and right motors:
private MotorSpeedController m_leftController = null; private MotorSpeedController m_rightController = null;
Then initialize the controllers with the following in the Init() function.
m_leftController = new MotorSpeedController(m_leftMotor, m_leftEncoder, 0, 0, 0, 0.002);
m_rightController = new MotorSpeedController(m_rightMotor, m_rightEncoder, 0, 0, 0, 0.002);
When we look at the documentation for the MotorSpeedController class, we see that there are a number of constructors that we can use. We have chosen the one that allows us to set the four parameters P, I, D and F. Figuring out the correct values for all of these is somewhat tricky and we don’t want to try and figure them out all at once. 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 500 units/sec. Thus we need a F term that, when multiplied by the max speed (i.e. 500) will result in full power being applied (i.e. a power of 1.0). That number is, of course 0.002, which is the number we are using. Also, since we see that the minimum power required to operate the motors is 0.2, we need to tell the MotorSpeeController this by adding the following two lines:
m_leftController.SetMinPower(k_minPower);
m_rightController.SetMinPower(k_minPower);
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.
The next 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 Init() function:
m_leftController.SetScale(k_maxSpeed); m_rightController.SetScale(k_maxSpeed);
And we define both k_minPower and k_maxSpeed at the top of the class as follows:
private static final int k_maxSpeed = 500; private static final double k_minPower = 0.2;
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 left, double right)
{
m_leftController.set(left);
m_rightController.set(right);
}
Your DriveSubsystem.java file should now look like:
package subsystem;
import commands.ArcadeDriveCommand;
import robotCore.AnalogEncoder;
import robotCore.I2CPWMMotor;
import robotCore.Joystick;
import robotCore.Logger;
import robotCore.MotorSpeedController;
import wpilib.Subsystem;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_leftEncoderNo = 1;
private static final int k_rightEncoderNo = 0;
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;
private static final int k_maxSpeed = 500;
private I2CPWMMotor m_rightMotor = null;
private I2CPWMMotor m_leftMotor = null;
private Joystick m_joystick = null;
private AnalogEncoder m_rightEncoder = null;
private AnalogEncoder m_leftEncoder = null;
private MotorSpeedController m_leftController = null;
private MotorSpeedController m_rightController = null;
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
setDefaultCommand(new ArcadeDriveCommand());
}
public void Init()
{
m_leftMotor = new I2CPWMMotor(k_leftMotorPin1, k_leftMotorPin2);
m_rightMotor = new I2CPWMMotor(k_rightMotorPin1, k_rightMotorPin2);
m_joystick = new Joystick(0);
m_rightEncoder = new AnalogEncoder(k_rightEncoderNo, true);
m_leftEncoder = new AnalogEncoder(k_leftEncoderNo, true);
m_leftController = new MotorSpeedController(m_leftMotor, m_leftEncoder, 0, 0, 0, 0.002);
m_rightController = new MotorSpeedController(m_rightMotor, m_rightEncoder, 0, 0, 0, 0.002);
m_leftController.SetScale(k_maxSpeed);
m_rightController.SetScale(k_maxSpeed);
Logger.Log("DriveSubsystem", 2, "Init()");
}
public void SetPower(double left, double right)
{
Logger.Log("DriveSubsystem", -1, String.format("SetPower: l=%.2f, r=%.2f", left, right));
m_leftMotor.set(left);
m_rightMotor.set(right);
}
public void ArcadeDrive()
{
double x = m_joystick.getX();
double y = m_joystick.getY();
Logger.Log("DriveSubsystem", -1, String.format("ArcadeDrive: x=%.2f, y=%.2f", x, y));
x = (x < 0) ? -(x * x) : (x * x);
y = (y < 0) ? -(y * y) : (y * y);
SetPower(y + x, y - x);
}
public AnalogEncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public AnalogEncoder GetRightEncoder()
{
return(m_rightEncoder);
}
public void SetSpeed(double left, double right)
{
m_leftController.set(left);
m_rightController.set(right);
}
}
Now we want to test this out and see graphically how the motors perform. Open your DriveSpeedTestCommand.java file. This time, we want to record the speed as a function of time. We will no longer need the m_currentPower variable, so we will delete it, and all references to it. In the initialize() function we will change the header of our log file to:
Logger.Log("DriveSpeedTest", 3, "Time,Left,Right");
We are going to drive at a constant speed at half power for this test so add the following line to the initialize() function.
Robot.m_driveSubsystem.SetSpeed(0.5, 0.5);
The execute() function should be changed as follows to eliminate the use of m_currentPower (we won’t be changing the speed for this test):
protected void execute()
{
Logger.Log("DriveSpeedTestCommand", -1, "execute()");
Logger.Log("DriveSpeedTest", 3, String.format("%d,%d,%d",
Logger.GetElapsedTime(),
Robot.m_driveSubsystem.GetLeftEncoder().getRate(),
Robot.m_driveSubsystem.GetRightEncoder().getRate()));
Robot.Sleep(40);
}
Finally since we are now using the speed controller to control the speed, we need to turn the motors off at the end using the SetSpeed function. Change the interrupted() line to:
Robot.m_driveSubsystem.SetSpeed(0, 0);
Your DriveSpeedTestCommand.java file should now look like:
package commands;
import robot.Robot;
import robotCore.Logger;
import wpilib.Command;
/**
*
*/
public class DriveSpeedTestCommand extends Command
{
public DriveSpeedTestCommand()
{
Logger.Log("DriveSpeedTestCommand", 3, "DriveSpeedTestCommand()");
// 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("DriveSpeedTestCommand", 2, "initialize()");
Robot.m_driveSubsystem.GetLeftEncoder().reset();
Robot.m_driveSubsystem.GetRightEncoder().reset();
Robot.m_driveSubsystem.SetSpeed(0.5, 0.5);
Logger.SetLogFile("DriveSpeedTest", "DriveSpeedTest.csv");
Logger.Log("DriveSpeedTest", 3, "Time,Left,Right");
Logger.ResetElapsedTime();
}
// Called repeatedly when this Command is scheduled to run
protected void execute()
{
Logger.Log("DriveSpeedTestCommand", -1, "execute()");
Logger.Log("DriveSpeedTest", 3, String.format("%d,%d,%d",
Logger.GetElapsedTime(),
Robot.m_driveSubsystem.GetLeftEncoder().getRate(),
Robot.m_driveSubsystem.GetRightEncoder().getRate()));
Robot.Sleep(40);
}
// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished()
{
Logger.Log("DriveSpeedTestCommand", -1, "isFinished()");
return (false);
}
// Called once after isFinished returns true
protected void end()
{
Logger.Log("DriveSpeedTestCommand", 2, "end()");
}
// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted()
{
Logger.Log("DriveSpeedTestCommand", 2, "interrupted()");
Robot.m_driveSubsystem.SetSpeed(0, 0);
}
}
Now deploy and run your program and press button 12 for a couple of seconds. Then get the new log file off the robot and plot it on a graph and you should see something like:
So we can see that when we set the speed to 50% we are getting a little about 230 units/second which is a little less than half of the max speed. However, this is probably close enough to the target for our purposes. Had this number been significantly off, then we would have wanted to adjust our F term either up or down to compensate.
Now 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.0004. In the DriveSubsystem.java file, change the following two lines:
m_leftController = new MotorSpeedController(m_leftMotor, m_leftEncoder, 0, 0.0004, 0, 0.002);
m_rightController = new MotorSpeedController(m_rightMotor, m_rightEncoder, 0, 0.0004, 0, 0.002);
Now when we run the program, we see that the robot moves rather jerkily. When we pull the data off the robot and plot it, we will see this in more detail:
So you can see that there a wild gyrations in the speed, but it is trying to settle in on the correct value of 250 units/sec. These gyrations mean that our I term is too large. Let’s set it to 0.0001 and try again. Doing so reveals this new graph:
As we can see this is much better. It does settle in quite nicely at the desired speed of 250 units/sec. However there is this huge spike at the beginning that we would very much like to get rid of. We could lower the I term even more, but that will not help as much as you think. The problem is that it takes about 0.25 seconds to reach the setPoint, and during that time the accumulated error term keeps growing and growing. So when it reaches the setPoint there is a too much power being added because of that accumulated error. What we really need to do is not apply the I term until we get close to the setPoint. Fortunately, there is a way to do that.
If we look at the documentation for MotorSpeedController, we find just the function we are looking for, SetIRange. This allows us to control when the I term is used. So add the following two lines to the Init() function of your DriveSubsystem class. This will make it so the I term does not kick in until the speed is withing 75 units/sec of the target.
m_leftController.SetIRange(75);
m_rightController.SetIRange(75);
Now run your program again and graph the result and you should see something like:
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:
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 DriveSubsystem.java file should now look like:
package subsystem;
import commands.ArcadeDriveCommand;
import robotCore.AnalogEncoder;
import robotCore.I2CPWMMotor;
import robotCore.Joystick;
import robotCore.Logger;
import robotCore.MotorSpeedController;
import wpilib.Subsystem;
/**
*
*/
public class DriveSubsystem extends Subsystem
{
private static final int k_leftEncoderNo = 1;
private static final int k_rightEncoderNo = 0;
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;
private static final int k_maxSpeed = 500;
private static final double k_minPower = 0.2;
private I2CPWMMotor m_rightMotor = null;
private I2CPWMMotor m_leftMotor = null;
private Joystick m_joystick = null;
private AnalogEncoder m_rightEncoder = null;
private AnalogEncoder m_leftEncoder = null;
private MotorSpeedController m_leftController = null;
private MotorSpeedController m_rightController = null;
public void initDefaultCommand()
{
Logger.Log("DriveSubsystem", 2, "initDefaultCommand()");
setDefaultCommand(new ArcadeDriveCommand());
}
public void Init()
{
m_leftMotor = new I2CPWMMotor(k_leftMotorPin1, k_leftMotorPin2);
m_rightMotor = new I2CPWMMotor(k_rightMotorPin1, k_rightMotorPin2);
m_joystick = new Joystick(0);
m_rightEncoder = new AnalogEncoder(k_rightEncoderNo, true);
m_leftEncoder = new AnalogEncoder(k_leftEncoderNo, true);
m_leftController = new MotorSpeedController(m_leftMotor, m_leftEncoder, 0, 0.0001, 0, 0.002);
m_rightController = new MotorSpeedController(m_rightMotor, m_rightEncoder, 0, 0.0001, 0, 0.002);
m_leftController.SetMinPower(k_minPower);
m_rightController.SetMinPower(k_minPower);
m_leftController.SetScale(k_maxSpeed);
m_rightController.SetScale(k_maxSpeed);
m_leftController.SetIRange(75);
m_rightController.SetIRange(75);
Logger.Log("DriveSubsystem", 2, "Init()");
}
public void SetPower(double left, double right)
{
Logger.Log("DriveSubsystem", -1, String.format("SetPower: l=%.2f, r=%.2f", left, right));
m_leftMotor.set(left);
m_rightMotor.set(right);
}
public void SetSpeed(double left, double right)
{
m_leftController.set(left);
m_rightController.set(right);
}
public void ArcadeDrive()
{
double x = m_joystick.getX();
double y = m_joystick.getY();
Logger.Log("DriveSubsystem", -1, String.format("ArcadeDrive: x=%.2f, y=%.2f", x, y));
x = (x < 0) ? -(x * x) : (x * x);
y = (y < 0) ? -(y * y) : (y * y);
SetSpeed(y + x, y - x);
}
public AnalogEncoder GetLeftEncoder()
{
return(m_leftEncoder);
}
public AnalogEncoder GetRightEncoder()
{
return(m_rightEncoder);
}
}
If you like, you can try and adjust the PID parameters further to see if you can get a better result. 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.