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 by automatically adjusting the power of the motors 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 ExampleCommand and ExampleSubsystem strings. Your result should look like:

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 need to create a member variable m_power to hold the current power setting. We are also going to want to record the speed of the motors so we will need an Encoder instance for the left and right encoders.

In the initialize() function we need to set the power to 0. Also we will be logging the motor speeds in the execute() function in a format that will allow us to import them into a spreadsheet. To make the spreadsheet easier to read we add a title row which labels each column.

Then in the execute() function we will set the drive power to the current power and then increase the power by 0.0025 each time we are called. We will use the getSpeed() function of the Encoder class to log the speeds of the two motors so that we can view them later. To accomplish this we change the execute() function as follows:

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 is to see exactly where the speed tops out at.

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

Your TestMotorSpeedCommand.java file should now look like:

Finally we want to set it so that button 3 on the joystick will run this command. Switching to the RobotContainer.java file, we add a declaration for m_button3:

and then configure it to run our TestMotorSpeedCommand when it is pressed:

Now run your program and connect and enable your robot. Before pressing the B3 button to run our command we want to clear the TERMINAL in VS Code. Unfortunately the only way I have found to do that is a little bit convoluted (if anyone can figure out a simpler way to do this, please let me know!). First right click on the word TERMINAL at the bottom of the screen. This will bring up the following menu:

Now ignore this menu and without closing it move the cursor down to the TERMINAL area and right click. This should then give you the menu:

Choose the Clear option from this menu.

Now press the B3 button on the Driver Station. The robot should drive forward slowly increasing it speed. When it has stopped, the logged information should be in the TERMINAL pane:

Now we want to select all of the text in this window so we can paste it into our spreadsheet. Unfortunately using the normal Windows shortcut CTRL + a does not seem to work for some reason. However we can us the same trick that we used to clear the TERMINAL. First right click on the word TERMINAL. Then, ignoring that menu, right click on the TERMINAL window and choose Select All. Then repeat this procedure and this time choose Copy.

Now that you have the text copied to the clipboard start up LibreOffice Calc. Then choose Edit / Paste and you should get the following dialog:

Make sure the Comma option is checked under the Separator Options and then click OK.

Then select the PowerLeft and Right columns (i.e. Dand F):

and click the Insert Chart icon at the top:

For the chart type choose the XY (Scatter) and Lines Only options as shown below:

Which should produce a graph similar to the one below:

This is a plot of the motor speed vs the power applied. There are several things we can see from looking at the graph. The first is that the motors will not even start running until the power reaches about 0.3 and tops out when the power reaches 1.0 (a power of 1.0 represents running the motor at full power). The second is that the left motor is less powerful than the right which is the reason the robot turns left. The last takeaway is that the max speed for the right motor is about 1200 and the max speed of the left motor is about 1150. If we want the robot to drive straight then the max speed for the whole robot will need to be limited to the slower motor (i.e. 1150). So let’s define a constant for that max speed:

Note that we have made k_maxSpeed public here. This is so we can later use it to log the target speed in the units of the encoders.

Note that these are the results for this particular robot. Your robot may differ and you should adjust these numbers accordingly. 

The SmartMotor class (which the PWMMotor class inherits) 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:


  • 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 of the right motor is about 1200units/sec.  Thus we need a F term that, when multiplied by the max speed (i.e. 1200) will result in full power being applied (i.e. a power of 1.0).  That number is 1.0/1200, which is the number we will use for the right motor. In a similar manner we compute that we should use 1.0/1150 for the left motor. Note that in both expressions, we 1.0 in the numerator rather than just 1. This is because in Java, if an expression contains only integers, then the result will be an integer thus 1/1200expressed as an integer is just zero.

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 terms, we add following two lines to the DriveSubsystem constructor:

Of course we must define k_Fleft and k_Fright:

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 that by calling setFeedbackDevice() for the motors in the DriveSubsystem constructor as follows:

Next we want 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 calling setMaxSpeed() in the constructor:

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:

Note that we must 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:

Your DriveSubsystem.java file should now look something like:

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 CalibrateSpeedCommand and, as before, copy over the code from ExampleCommand, renaming the the file name, and replacing ExampleCommand and ExampleSubsystem as you have done before.

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. Since we will be logging the motor speeds, we will need access to the left and right encoders so we declare member variables to hold those values and initialize them in the constructor:

In the initialize() function we will simply use our new setSpeed(…) function to start the robot forward at 0.60 speed. Since we will also be logging the speed, we log the title row for our CSV file.

And where we have defined k_speed above.

In the execute() function we want to log the current time, power, and speeds of the two motors.

Note that we are also logging the target speed and we need to multiply k_speed by the k_maxSpeed defined in the DriveSubsystem so that the units will match those returned by the encoder’s getSpeed() function.

Now for this command we are going to make it run as long as we hold down button 4 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 4 on the joystick, this command will be interrupted and it’s end() function will be called. At that point we need to turn the motors off.

Your CalibrateSpeedCommand.java should now look like:

Now connect our test button 4 to our new CalibrateSpeedCommand by adding a m_button4 to RobotContainer:

And configuring it so that our new command will run while the button his held down:

Your RobotContainer.java file should now look like:

Now run your program, and connect and enable your robot. Then, as before, clear the TERMINAL window in VS Code. Then switch to the Driver Station and hold the B4 button down for a few seconds. Go back to VS Code and copy all of the text in the TERMINAL window and paste it into LibreOffice Calc. As before, delete all of the rows up to the header line. Now select the Target Speed, Left Speed, and Right Speed columns and plot the data. This time, however, choose the Line / Lines Only option as shown below:

This should result in a graph which looks something like this:

The blue line represents the target speed and the red and yellow lines are the actual speeds of the motors. We can see from the graph that the motor speeds are too low so we will need to increase the F term for both motors. To get an approximation of how much we need to change them we can multiply the current value by the ratio of the target speed to the actual speed. We can then multiply that by the current F values to get new estimated F values. We can see that the target speed is 690 and for both motors the current speed is around 450. This means we need to multiply the current F values by 690/450 or 1.5:

Remember that these numbers are for this particular robot only. Your robot may behave slightly differently and you should adjust your number accordingly.

Now run your program again and copy the data to LibreOffice Calc and you should get a graph something like:

This is now a bit high so we are going to reduce the F terms a little bit. Keep adjusting the F values until the motors are running close to the target speed. Don’t worry about getting it exact, the other terms will compensate for small errors. Here are the numbers I used for my robot:

With these numbers I got the graph:

Now this is much better. Remember that we do not need this to be perfect. In fact it is pretty much impossible to get it perfect using only the F term, that is what the other terms are for. But do notice that even now the robot is driving a lot straighter than it was before. Also, note that I ended up using the same value of F for both the left and right motors. You may need to use different values for your robot.

Now that we have the F term established we are going to work on the P term. The P stands for proportional. What this means is that power is added or subtracted depending on how far off the actual speed is from the target speed. Determining the first guess for the P term is somewhat difficult so we are just going to try something and see how it works. Let’s try 0.0002 (in this case we won’t need to use different values for the two motors).

To set the P term we add the following to the constructor:

And defining k_P as follows:

Using this value, we get the following graph:

We can see that this graph already looks better than the previous one that did not have a P term. Now what we want is the largest value of P that does not make the robot unstable. If P is too large, the robot will start to oscillate and we don’t want that. So what we are going to do is keep increasing the value of P until we see it become unstable, and then back off.

If we try a P value to 0.005 we will get the following graph:

In my case, this makes the speed unstable as you can see. In your case, this might not happen at a value of 0.005 and if it doesn’t, you should continue to increase the P value until it does become unstable and then revert back to the last stable value. In my case I will be using 0.002 for my value of P

Now adding the P term helps a lot but if the robot were to encounter some additional resistance (such as going up a ramp) it would not be able to completely compensate. For that we need the I term.

The I stands for integral. What the PID controller does is run in a loop. Each time through the loop it computes an error term which is the difference between the set point and the actual speed. The P term then multiplies that error term to make it’s adjustment. However, the PID loop also keeps track of a total error, and each time through the loop the current error term is added to the total error. The I term is then multiplied by the total error and that is added to the power equation. So, lets say, that the speed is too low because the robot is going up a ramp. As long as the speed is too slow, the total error will continue to increase which will cause the power applied to continue to increase until there is enough power to maintain the correct speed.

Once again choosing an initial value for the I term is difficult and we will have to make a guess. Let’s start with 0.001. We set the I term like this::

Defining k_I as follows:

There is one last thing we need to do before we run our program. Using the I term is great but really only works when our speed is close to the target value. When we first start the robot, it takes some time for the robot to get up to speed and during that time we build up a large total error term. This then causes us to overshoot our target. Eventually it settles down to the correct value but we don’t want the overshoot. The way to prevent this is to only use the I term when the current speed is close to the target speed. We set this range by using the setIZone() function. Now we need to set the zone large enough that it encompasses the natural oscillation of the motors. Looking at the graphs we can see that this range is around 150. Set the I Zone as follows:

Defining k_IZone as:

Now if we run the program we get the following graph:

This looks pretty good. Now we want to do the same thing we did with the P term and keep increasing the value until the robot becomes unstable. For my robot, I found that a P value of 0.002 produced the following.

We can see that we have introduced some stability so I set the I term back to 0.001. Not that for your robot you may need a different I term.

Your DriveSubsystem.java file should now look something like:

Remember that the actual values for the FI, and P terms for your robot may differ from the ones shown here.

Before we wrap up, let’s see how it behaves at another speed.  Change the k_speed constant in CalibrateSpeedCommand make a graph for 0.75 speed. You should see something like:

We can see that it looks pretty good. There is a bit of an overshoot at the beginning but it is not significant so we will not muck with the PID values. The fact is, no single set of PID terms is going to work for all speeds. When tuning your robot you should tune it for the speed that is most important. You should then check other speeds and adjust the PID parameters only if there is a serious problem.

Let’s make one last change before we move on.  Change the call to setPower in the ArcadeDrive() function to setSpeed.  This will allow us to control the robot by speed even during teleop which can make it easier to control your robot, especially if the two motors run at different speeds for the same power setting. Your ArcadeDriveCommand.java file should now look like:

Next: Drive Straight