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.

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:

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

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.

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.

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

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:

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.

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

Your TestMotorSpeedCommand.java file should now look like:

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:

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.


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:


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:


Be sure to choose Comma as the Separator Options:


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


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


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:


  • 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:

And, of course we must define k_F as follows:

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:

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:

Defining k_maxSpeed at the top of the class as follows:

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:

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:

You DriveSubsystem.java file should now look 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 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.

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.

Of course defining k_speed:

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.

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.

Your TestSpeedControlCommand.java file should now look like:

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

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:

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:


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:



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:

And define k_P as follows:

Your DriveSubsystem.java file should now look like:

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


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:

Of course declaring the constants as follows:

Your DriveSubsytem.java file should now look like:

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:


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:

Run your program and pull and graph the log file 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:



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:

Next: Drive Straight

Pierre Pierre, who replica handbag 185cm tall, gucci replica handbags the "Diamond King" in his hermes replica handbags . He has been selected as one of the handbag replica most beautiful 50 people in the "People" magazine. It is also known as replica handbags most elegant and quiet in the world. Unforgettable prince.