Wheel Encoders

So far we have been controlling the motors by setting their power. Unfortunately, this is not ideal because the power only indirectly controls the speed, which is what we are really interested in. As we have already seen, our robot does not exactly drive straight when we apply the same power to both wheels. This is because differences in the motors and the way they are mounted cause them to run at slightly different speeds when the same power is applied.

What we need is some way to tell how far the wheels have turned and how fast. Each of the wheels on this robot have an encoder which will do just that. We will use the encoder to control the distance the robot moves as well as it’s speed.

For our first exercise, we are going to create a new command, similar to our DriveForTimeCommand, that will drive the robot forward a fixed distance, rather than a fixed time.

The encoders that we are using are handled by the Encoder class. We will need to create a separate instance of this class for the left and right motors. The DriveSubsystem will eventually need the encoders so we will be adding these to that class. Consulting the Encoder class, we see that the constructor take a encoder type and two integer parameters which specifies the Arduino pins to which they are connected. For this robot the encoder type is Quadrature. The left encoder is connected to pins Q1_INT and Q1_DIR, while the right encoder is connected to pins Q2_INT and Q2_DIR. Open the DriveSubsystem.java file and add the following at top of the class.

When you import the Encoder class be sure to select the one from RobotCore. Also note that once again we are declaring constants for the encoder pins rather than simply using the numbers directly in the call to the constructor.

Now the way these encoders are wired we will discover that, for the left encoder, when the wheel moves in the forward direction, the encoder will count up. However the right encoder will count down when the wheel moves forward. Hence we want to reverse the direction of the right encoder. We do this by adding the following to the DriveSubsystem constructor:

The call to setInverted(true) tells the encoder to reverse it’s direction.

Now we are going to need access to these encoders from our Command classes, so we will also create functions to retrieve the left and right encoders:

Note that we are actually returning a copy of the Encoder rather than the one saved by the DriveSubsystem. This is so that the user of the copy is free to reset() the Encoder without affecting others who may be using the Encoder.

Now it is time to create our new command which we will call DriveForDistanceCommand. Go ahead and create a new class under the commands folder. This time instead of using the ExampleCommand as our starting point, copy the contents of DriveForTimeCommand.java file into your newly created DriveForDistanceCommand.java file. Then rename the file and replace all instances of DriveForTimeCommand with DriveForDistanceCommand.

Since we will not be using a Timer for this class, remove all references to the Timer. This will involve removing the time parameter from the constructor and having isFinished() function return false.

This will give us a better starting point. Your new file should look like:

Next we want to change our constructor to take a distance instead of a time. We also need to get the encoders from the DriveSubsystem. To measure the distance we can use either of the encoders but for this command we will use the left encoder.

Again, when you import the declaration for Encoder be sure to choose the one from robotCore.

In the initialize() function we need to reset the Encoder so that it counts from zero each time we run this command. Remember that the encoder we receive from the DriveSubsystem is our own copy so resetting it will not affect the DriveSubsystem or any other commands with use the encoders.

The last thing we need to do is change the isFinished() function to return true when the distance is greater than or equal to the target distance:

Your DriveForDistanceCommand.java file should now look like:

Now we will configure the RobotContainer class to tie this new command to button 2 on the joystick. In RobotContainer.java we first add a declaration for m_button2:

Then we connect it so when this button is pressed we will run the DriveForDistanceCommand:

Note that the distance that we are passing in are in somewhat arbitrary units. The Encoder counts are somewhat arbitrary as they depend on the particular motor/encoder and the size of the wheel. We have chosen 3000 here just to test this command. Later we will perform a conversion which will allow us to use real world units such as inches.

Your RobotContainer.java file should now look like:

Now run your program and click the B2 button. You should find that your robot will drive a short distance and then stop. Also note that since we are now using distance rather than time, we can change the speed and the robot should still move the same distance. Try it out and see.

Next: Speed Control