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 file and change the SetPower calls to SetSpeed calls. Your file should now look like:

Open up the file and change the test button (2) back to run the DriveForDistanceCommand and also increase the distance it travels to 4000 units so we can better see how it behaves. So your file should now look like:

Now if you deploy and run the (remember that this command is tied to button 2 on the joystick), you will see that once the robot gets going, it drives pretty straight. However while it is starting up, it turns 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. To do this we are going to need to access both the left and right encoders (currently we are only using the left encoder to measure the distance the robot has traveled). Now we could use the left and right encoders from the DriveSubsystem directly like we did before, but since we are going to be wanting to reset the encoders from time to time it would be nice to have our own copies so that doing so does not affect others who might need the encoders.

To this effect, we are going to create two new member variables that will hold copies of the left and right encoders:

Now we need to initialize these variable somewhere. We don’t want to do it in the constructor for this class because the encoders for the DriveSubsystem may not have yet been created when the constructor is called. Therefore we will do it in the initialize() function, when this command is invoked.

Note that when we call the GetLeftEncoder() function for the DriveSubsystem, we call it’s Clone() function. This will make a copy of the encoder. This copy will correspond to the same physical encoder, but will have it’s own zero point, so resetting this encoder will not affect the zero point of the encoder used by the DriveSubsystem.

Not also, that after obtaining the copies of the left and right encoders, we reset their current values to zero (we also removed the previous call to Robot.m_driveSubsystem.GetLeftEncoder().reset()).

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:

How can we use these numbers to keep the robot driving straight? Consider what we get if we compute the difference between these two numbers:

Now if the robot is driving straight then 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:

This is not quite what we want, however. Remember that the speed value 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. So what we need to do is to scale the deltaDistance by some fractional constant. Since we know that a full revolution of the wheel is about 1000 distance units, lets start with a scale factor of 0.001:

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:

Finally, since we are now using local copies of the encoders we need to change our isFinished() function to use that copy.

Your 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. Since the robot currently is taking a little longer than we like to correct it’s path, try increasing the value of k_scale to 0.002. Note that if you make this number too large, the robot will over correct and start to swerve. Making it even larger can simply result in the robot quickly turning back and forth. Feel free to experiment with some other numbers and see what happens.

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 32 inches. This means that 4000 encoder ticks is equal to 32 inches. So if we want to pass in the distance in inches, we will need to multiply that by (4000 / 33) to get encoder units. Change the DriveForDistanceCommand constructor to perform this calculation:

Now change your OI call to set the distance to drive to be 20 inches.

Now test out your program and see that the robot drives 20 inches.

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

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.