AutomaticTurnCommand

No driver is ever perfect, and all will definitely appreciate assistance with aiming the game pieces. With the limelight, this is easy to implement. Limelight.getTargetXAngle(), can determine the amount needed to turn for alignment to the target.

Here is the class that would be used to have the robot turn towards the target:

public class AutomaticTurnCommand extends DrivetrainDriveCommand {

    @Override
    protected void initialize() {
        setInterruptible(false);
    }

    @Override
    protected void setTurn() {
        // Get Gamepad Input
        super.setTurn();

        // If Using CV
        if(Limelight.hasValidTarget()) {
            // Get Turn Div from Smart Dash Board
            double turnDiv = SmartDashboard.getNumber("TURN_DIV", RobotMap.CV.TURN_DIV);
            double moveTurnMult = SmartDashboard.getNumber("MOVE_TURN_MUL", RobotMap.CV.MOVE_TURN_MUL);

            // Take The Square Root of the X Angle
            double turnDelta = Limelight.getTargetXAngle();
            turnDelta = Math.signum(turnDelta) * Math.sqrt(Math.abs(turnDelta));

            // Increase Turning if robot is moving faster
            turnDelta *= Math.max(moveTurnMult * speed, 1);

            // Scale the Turn Delta
            turnDelta /= turnDiv;

            // Add Turn Delta to Turn
            turn += turnDelta;
        }
    }
}

This extends the DrivetrainDriveCommand class, which allows it to drive the robot by itself. The inclusion of super.setTurn() still allows the usage of the controller inputs to add to the limelight's turn value.

Limelight.hasValidTarget() determines if there is a target to aim at, or if the Limelight is working at all.

First, the constants used for turning the robot must be obtained.

double turnDiv = SmartDashboard.getNumber("TURN_DIV", RobotMap.CV.TURN_DIV);
double moveTurnMult = SmartDashboard.getNumber("MOVE_TURN_MUL", RobotMap.CV.MOVE_TURN_MUL);

SmartDashboard can be used to quickly change these values during testing, but it is good practice to update these constants in RobotMap or any similar constants file.

Current Constants and Their Purposes:

1. TURN_DIV

   • When the limelight returns the angle that the target is in its camera, it returns a value between -27 and 27 degrees. However, the drivetrain only accepts turn values from -1 to 1. In order to convert the angle to an accepted value, it must be divided by this constant.

   • How To Tune:

     • If the robot turns to the target too slowly, or does not reach it at all, decrease this value.

     • If the robot overshoots or oscillates, increase this value.

   • Recommended Value: 16 to 24 (When used after MOVE_TURN_MUL and finding the square root)

2. MOVE_TURN_MUL

   • If the robot is moving faster, turning has less of an effect. This constant increases the amount to turn.

   • How To Tune:

     • Increase this constant if the robot crashes into the side of the target when moving. Decrease either based on personal preference or if oscillation is present.

   • Recommended Value: 3 to 8; Alfred uses 6.3. Edwin is from 7-9.

3. Limelight.X_ANGLE_SHIFT

   • In newer versions of Limelight.java, there is a variable called Limelight.X_ANGLE_SHIFT which works as an offset. Make sure this updated before tuning.

   • How To Tune: Drive up to the target using Autodrive. If it turns too far left, increase this value; if it turns too far right, decrease this value. THIS IS ONLY USED WHEN THE INITIAL CROSSHAIRS ARE OFF, you should probably tune the crosshairs, but this is a good last-minute fix.

How Automatic Turn Works - Implementation of these Constants

First, get the x offset from the Limelight:

double turnDelta = Limelight.getTargetXAngle();
turnDelta = Math.signum(turnDelta) * Math.sqrt(Math.abs(turnDelta));

The offset is stored in the variable turnDelta so that the AutomaticTurn value can be modified without changing the turn value of the entire robot.

Then, take the square root of turnDelta. If the offset happens to be negative, the square root will produce an imaginary number. Remedy this by finding the square root of the absolute value of turnDelta and reverting the result to the original sign using Math.signum(turnDelta). The square root of the X offset is used because the motors are less likely to stall at smaller values.

After that, multiply turnDelta by MOVE_TURN_MUL if the robot is moving too fast.

turnDelta *= Math.max(moveTurnMult * speed, 1);

Math.max is used to ensure that if MOVE_TURN_MUL * speed is below 1, the robot is still able to turn.

Lastly, scale turnDelta and add it to the drivetrain's turn value:

turnDelta /= turnDiv;
turn += turnDelta;