# Drive to position part 2

Now that we have the robot pointing in the correct direction, we need to create the command which will make it drive to the target point.  Let’s call this new command DriveToPointCommand.

Like for the RotateToPointCommand we need to change the constructor to pass in the target X and Y position. We also need to require the DriveSubsystem.

In the execute() function, we want to use the current position and the target position to determine what angle we need to be driving at.  Then based on the deviation from that angle, we want to adjust the left and right motors to make course corrections.  We also want to control the speed based on how far away from the target we are, moving slower as we approach the destination.

We are also going to need to determine when we have reached the target.  Just looking at the distance is likely to cause us problem. Unless we are headed exactly at the destination, we can easily slip by.  So we are going to use a combination of the distance and the delta angle.  If the distance is within an acceptable range, we will consider ourselves done.  Alternately, if the delta angle becomes too great (indicating that we are passing the target), we will also stop.

Taking these things into account, your execute() function should look like:

We will use the member variable m_done to indicate whether we are at the target or not.  Here we see that we are going to set it to true if either the distance to the target is less than k_minDistance, or the delta angle is greater than k_maxAngle.

We must define m_done and the various constants as follows:

We will also want to set m_done to false in our initialize() function:

In our isFinished() function we need to return true when the robot has reached the target.

Finally, in our end() and interrupted() functions we need to stop the motors:

Your DriveToPointCommand.java file should now look like:

Now we want to connect this new command to our test button.  However, this command will only work if the robot is pointed at the target location. What we really need to do when the test button is pressed is to first call our RotateToPointCommand and then this new DriveToPointCommand.  Fortunately, the command processor provides a way to do this.

We can execute a group of commands by creating an instance the CommandGroup class and then add the command that we want executed by adding the following code to our OI class:

First we create the group, then add our commands to it, and set it so that the group is executed when our test button is pressed. Note that we have chosen an arbitrary point (0, 40) and are going to have the robot turn to face that point and then drive to it.  Your OI.java file should now look like:

Now deploy and run your program. Remember that when you start the robot, it must be pointed directly at the tower so that the gyro zero point is set correctly.  Then you should be able to move the robot to any point on the field, and when you press the test button (12), the robot should turn and drive to a point 40 inches directly in front of the tower.

Before we move on, let’s create a group command which will cause the robot to drive and shoot from anywhere on the field.  Let’s connect this command to button 4 on the joystick:

Here we create a new group and add commands that will make the robot drive to position (0, 40) in front of the tower (as before), then turn to face the tower, and finally run the AutoShootCommand to make the shot.  We, of course, need to define this new button:

At this point your OI.java file should look like:

Now deploy and run your program and you should find that you can place the robot anywhere on the field, and it will line up with the tower and make the shot.

Next: Driver station control

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.