Turn Command

Next we are going to create a command called TurnCommand which will allow us to turn the robot in place by a specified angle. You should know enough now to create the framework for this command. We will want to specify the speed to be used in the turn as well as the angle so create the constructor for your class so that it takes two double parameters, the first to specify the speed and the second to specify the angle. After you have created the new command, compare you code to the code below.

.

.

.

Did you create some member variables (e.g. m_speed and m_angle) to store the speed and angle that are passed in on the constructor?

Now we are going to use the encoders to measure the turn. Once again, we will want to use copies of the encoders like we did in our DriveForDistanceCommand. Add code to declare and initialize the left and right encoder:

.

.

.

.

Now in the initialize() function we also want reset the encoders and turn the motors on.

To make the robot turn, we need to set the speed to the left and right motors opposite, and we need to set the signs so that it turns in the correct direction. Let’s say that a positive angle should make the robot turn to the right (clockwise) and a negative angle should make the robot turn to the left (anticlockwise). See if you can add code to your initialize() function to accomplish this and then check your result with the one below.

.

.

.

.

Now we need to add some code to the isFinished() function to return true when the robot has turned by the requested angle. Add the following line to the isFinished() function:

Note that delta will be positive if the robot is turning right (i.e. the left wheel is moving forward and the right wheel is moving backward), and negative if the robot is turning left. The magnitude of delta will also be a measure of how far the robot is turned. So lets return true when the magnitude of delta exceeds the magnitude of the angle.

Note that we are using the absolute value of both delta and m_angle in the comparison since they can both be negative.

Remember to turn the motors back off in the end() function. Your TurnCommand.java file should now look like:

For now we will pass in an angle in the encoder units and see how far the robot turns (later we will find the conversion factor so we can enter our angle in degrees). Configure the RobotContainer class so that this command will be executed with 0.3 speed for and angle of 1500 when we press button 5 on the joystick. When you have made that change, compare it to the code below.

.

.

.

.

Now deploy and run your program and see how far the robot turns. Try and find a number to replace the 1500 value that will make the robot turn exactly 360 degrees.

.

.

.

.

In my case I found that approximately 1850 (4700 for the blue motors) encoder units will make the robot turn 360 degrees. This means that if the input angle is in degrees, we need to multiply it by (1850.0/360) or (4700.0/360) for the blue motors to get the number of encoder units that we need. NOTE that the value for your robot may be different, but the math is the same! Implementing this in the code we get:

Where I have defined k_ticksPerDegree as:

Note that I have used 1850.0 rather than just 1800 because I need to force it to use floating point when evaluating the expressiong.

Now change the line in the RobotContainer to turn the robot by 180 degrees:

Before moving on, verify that if you specify a negative angle the robot will turn the other way:

You may find that the robot does not turn exactly the desired 180 degrees. This is because the accuracy of the turns using this method is poor because of wheel slippage and other issues. If the robot had a gyroscope, we could use that for more accurate turns, but since it does not, we will need to live with the inaccuracies.

Next: Combining Commands