Drive Straight

Now that we have the ability to control the speed of the motors let’s change our DriveForDistanceCommand so to use speed rather than power to make the robot drive more straight. Open your DriveForDistanceCommand.java file and change the setPower calls to setSpeed calls. Also since we are now using speed rather than power let’s rename the power parameter to speed. The best way to do this is to use VS Studio’s Rename function.

Right click on the parameter power (1) and choose Rename Symbol (2),

then change the name from power to speed. Note that this changes all instances in your program automatically.

Do the same for the member variable m_power, changing it to m_speed. And update the comments to reflect this, if needed!

Your DriveForDistanceCommand.java file should now look like:

Now if you deploy and run the code (remember that this command is tied to joystick button 2), you will see that once the robot gets going, it drives pretty straight. However while it is starting up, it may turn a little bit. This is because, as we saw from the speed graphs in the previous chapter, that the motors do not start up quite in sync. What we want is a way to measure if the robot has turned and compensate the power so that it drives in a straight line.

To do this, we are going to change our DriveForDistanceCommand to use both the left and right encoders to keep the robot driving straight. Currently we are only using the left encoder to measure the distance the robot has traveled, so we need access to the right encoder.

Add a declaration for m_rightEncoder and initialize it in the constructor:

In the initialize() function we need to reset the right encoder:

Now in the execute() function we are going to adjust the speed of the motors to keep the robot driving straight. The first thing we want to do is to get the current left and right encoder values and compute the difference between the two:

If the robot is driving straight, deltaDistance should be zero because the left and right wheels have turned by the same amount. It is, therefore, our goal to keep this deltaDistance as close to zero as we can at all times. If deltaDistance is positive, then this means that the right wheel has turned farther than the left and we need to slow it down and speed up the left. Conversely, if deltaDistance is negative then the left wheel is running too fast and we need to slow it down and speed up the right. Conceptually we want to subtract the deltaDistance from the speed of the right motor and add it to the speed of the left motor. Something like:

But remember that speed should be in the range of -1.0 to +1.0. So if the difference between the left and right distance is greater than or equal to one, it will slam the robot to the left or right at full power which is not what we want.

To prevent this, we scale the deltaDistance by some fractional constant. Let’s start with a scale factor of 0.005:

Once again we are defining the scale factor as a constant k_scale which we define as follows:

The completed execute() function should now look like:

Your DriveForDistanceCommand.java file should now look like:

Now deploy and run your program. You should see that the robot does a better job of driving straight. The size of the k_scale factor will control how quickly the robot corrects it’s direction. Like we did with the PID tuning for the drive motors, we would like k_scale to be as large as possible without creating any instability. In my case I was able to increase it to 0.01 but larger than that cause it to oscillate.

There are other mechanisms that you can use to keep your robot driving in a particular direction. For example, if your robot has a gyroscope sensor which reports it current orientation, you can use that sensor in a similar manner to drive a straight line. You would adjust the left and right motor speeds based on how far the robot is deviating from the desired direction. This mechanism has the advantage on not being affected by any wheel slippage that might occur, which would cause errors when using our wheel encoder method.

Before we move on, let’s make one more change to our DriveForDistanceCommand. Specifying the distance in encoder units is OK, but it would really be nice if we could use real world units like inches instead. To do that we will need to compute a conversion factor that will convert inches into the encoder units.

Set the robot up to run the DriveForDistanceCommand again, but this time put a piece of tape on the floor to mark it’s starting position. Then run your program and mark the position where the robot ends and measure the distance it traveled using a tape measure.

You should find that the robot moves about 42.5 inches (16.0 inches for the blue motors). This means that 2000 encoder units is equal to 42.5 inches. We want to pass in the distance in inches (because those are distance units we all understand), so in the code we will need to multiply desired inches by (2000/42.5) or (2000/16.0) for the blue motor to get encoder units (needed to command the Encoder/Motor objects) . Change the DriveForDistanceCommand constructor to perform this calculation:

Where, in we defined k_ticksPerInch as:

Finally change your RobotContainer.java file to specify that the robot drive for 30 inches when button 2 is pressed, and give it a try.

One final note. This command, as written, will only drive the robot forward. If you attempt to drive backwards by setting the speed negative, you will find the robot never stops. It is left as an exercise for the reader to fix the program so that it can successfully drive either forward or backward.

Next: Turn Command