Drive to position

Now that we have a Navigator class which we can consult to give us the robot’s position and orientation, we want to create a way to drive to a specific point on the field.  We will use this in two ways.  First it will enable us to create a command which will move the robot into position to make a shot from anywhere on the field. Secondly, we will be able to use the Robot Mapper program on the driver station to control the robot.

We are going to accomplish this by creating two new commands.  The first command will rotate the robot to a specific location on the field.  The second command will drive to that location. Let’s call the first command  Go ahead and create the framework for this new command and connect it to our test button 12 on the joystick.

In the constructor, we will need to pass in the destination point as follows:

Note that this command will also require the DriveSubsystem so we ask for that as well.  The member variables m_targetX and m_targetY will be used to store the target location and must be delcared:

Note that since your constructor requires an X and Y value specified, when you create the instance of this class for your OI class, you will need to pick a location.  Let’s use (0, 0) which should point the robot to the base of the tower.  This line in your file should look like:

When the initialize() function is called, we will need to compute the angle to which we must turn to point at the target location.  Using trigonometry and the robot’s current position, we can calculate this angle as follows.

Declaring the member variables m_targetAngle and m_turnLeft as follows:

Note that we also determine whether the robot will be turning left or right.  We do this by computing the difference between the current yaw and the targetAngle.  If this value is greater than zero, then we will need to turn left, otherwise we return right.  Since we will need to compute this deltaAngle in multiple places, we will create a function, GetDeltaAngle which we will use.  This will allow us to keep this calculation in one place in case we need to modify it later.

Here we compute the difference between the current robot angle (yaw) and the target angle.  To make things easier, we want to force this angle to be between -180 and 180 degrees and we have created a function, NormalizeAngle, which does just that.  Rather than implementing this function in this command we are going to add it as a utility function to our Navigation class thus enabling us to use it for other command that we may create.

We make this function static so we can access it even if we don’t have an instance of the Navigation class handy.

Now back in our RotateToPointCommand class we need to change the execute() function to set the power to the motors to make the robot turn.  We will want to adjust the power depending on how var from the target angle the robot is:

We set the speed based on the current deltaAngle, subject to a minimum and maximum speed and then set the motors according to which direction we are turning.  The associated constants are defined as follows:

To get the deltaAngle we are once again calling GetDeltaAngle, but because we don’t have the robot’s position handy here we are creating a second version which will first obtain the position:

Next we need to change isFinised() to return true when the robot has reached the correct angle:

Note that the test differs depending on whether we are turning left or right.

Finally, once the turn is complete, we need to turn the motors off in the end() and interrupted() functions:

Your file should now look like:

Your file should look like:

and your file should now look like:

Now go ahead and test your program.  Remember that your robot must start facing the tower to get the zero angle properly set.  Then rotate the robot away from the tower and press button 12.  The robot should turn to face the tower.

Next: Drive to position part 2


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.