Next we are going to create a new function called turn(…) which will allow us to turn the robot in place by a specified angle. We will want to specify the speed to be used in the turn as well as the angle so create the function 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 function’s basic framework, compare you code to the code below.
.
.
.
|
1 2 3 4 5 |
public Command turn(double speed, double angle) { return run(coroutine -> { }).whenCanceled(this::stop).named("turn"); } |
The first thing we want to do is to reset the encoders:
|
1 2 |
m_leftEncoder.reset(); m_rightEncoder.reset(); |
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 left (anticlockwise) and a negative angle should make the robot turn to the right (clockwise). See if you can write code to accomplish this and then check your result with the one below.
.
.
.
.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
public Command turn(double speed, double angle) { return run(coroutine -> { m_leftEncoder.reset(); m_rightEncoder.reset(); if (angle > 0) { setSpeed(-speed, speed); // Turn left } else { setSpeed(speed, -speed); // Turn right } }).whenCanceled(this::stop).named("turn"); } |
Now we need to we need a loop that will end when the robot has turned the desired distance. We can get a measure of how far the robot has turned by taking the difference between the left and right encoder positions and we want to loop as long as this difference is less than the requested angle. Now the sign of that difference will depend on the direction of rotation we can eliminate that variable by taking its absolute value. The the while loop that we need will look something like:
|
1 2 3 |
while (Math.abs(m_leftEncoder.get() - m_rightEncoder.get()) < Math.abs(angle)) { coroutine.yield(); } |
Remembering to turn the motors back off at the end your turn(…) function should now look like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 |
public Command turn(double speed, double angle) { return run(coroutine -> { Logger.log("turn", 1, "run()"); m_leftEncoder.reset(); m_rightEncoder.reset(); if (angle > 0) { setSpeed(-speed, speed); // Turn left } else { setSpeed(speed, -speed); // Turn right } while (Math.abs(m_leftEncoder.get() - m_rightEncoder.get()) < Math.abs(angle)) { coroutine.yield(); } stop(); Logger.log("turn", 1, "end"); }).whenCanceled(this::stop).named("turn"); } |
As before, the encoder return distance in encoder ticks not degrees like we did for distance we need to compute the conversion factor. To do this we will first pass in an angle in the encoder units and see how far the robot turns. Add a line in the TeleopOpMode class to run this command with 0.3 speed for and angle of 1000 when we press button 5 on the joystick. Your TeleopOpMode() should now look like:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 |
public TeleopOpMode(Robot robot) { Logger.log("TeleopOpMode", 2, "TeleopOpMode"); robot.m_driveMechanism.setDefaultCommand(robot.m_driveMechanism.arcadeDrive( () -> robot.m_cmdJoystick.getX(), () -> robot.m_cmdJoystick.getY())); robot.m_cmdJoystick.getHID().button(0).onTrue(robot.m_driveMechanism.driveForTime(0.75, 2)); robot.m_cmdJoystick.getHID().button(1).onTrue(robot.m_driveMechanism.driveForDistance(0.75, 15.0)); robot.m_cmdJoystick.getHID().button(2).onTrue(robot.m_driveMechanism.testMotorSpeed()); robot.m_cmdJoystick.getHID().button(3).whileTrue(robot.m_driveMechanism.calibrateSpeed()); robot.m_cmdJoystick.getHID().button(4).onTrue(robot.m_driveMechanism.turn(0.3, 1000)); } |
Now deploy and run your program and see how far the robot turns. Try and find a number to replace the 1000 value that will make the robot turn exactly 360 degrees.
.
.
.
.
In my case I found that approximately 1050 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 1050.0/360 to get the number of encoder units that we need.
First let’s declare a constant at the beginning of our DriveMechanism class to convert from ticks to degrees. Note, once again, that we use 1050.0 rather than 1050 to ensure that floating point is used in the calculation.
|
1 |
private static final double k_ticksPerDegree = 1050.0 / 360; |
Then change our turn() function to convert from an input of degrees to ticks:
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 |
public Command turn(double speed, double angleInDegrees) { return run(coroutine -> { double angle = angleInDegrees * k_ticksPerDegree; Logger.log("turn", 1, "run()"); m_leftEncoder.reset(); m_rightEncoder.reset(); if (angle > 0) { setSpeed(-speed, speed); // Turn left } else { setSpeed(speed, -speed); // Turn right } while (Math.abs(m_leftEncoder.get() - m_rightEncoder.get()) < Math.abs(angle)) { coroutine.yield(); } stop(); Logger.log("turn", 1, "end"); }).whenCanceled(this::stop).named("turn"); } |
Now change the line in the RobotContainer to turn the robot by 180 degrees:
|
1 |
robot.m_cmdJoystick.getHID().button(4).onTrue(robot.m_driveMechanism.turn(0.3, 180)); |
Before moving on, verify that if you specify a negative angle the robot will turn the other way:
|
1 |
robot.m_cmdJoystick.getHID().button(4).onTrue(robot.m_driveMechanism.turn(0.3, -180)); |
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.