AutomaticDriveCommand

AutomaticDrive is a command that drives up to the target and turns at the same time.

public class AutomaticDriveCommand extends AutomaticTurnCommand {

    @Override
    protected void setSpeed() {
        super.setSpeed();
        if (Limelight.hasValidTarget()) {
            double targetXValue = Math.abs(Limelight.getTargetXAngle());
            targetXValue /= SmartDashboard.getNumber("SPEED_WHILE_TURNING", RobotMap.SPEED_WHILE_TURNING);

            double accel = Limelight.MAX_X_ANGLE - targetXValue;
            accel /= Limelight.MAX_X_ANGLE;

            accel *= SmartDashboard.getNumber("AUTOMATIC_DRIVE_SPEED", RobotMap.AUTOMATIC_DRIVE_SPEED);

            speed += accel;
        }
    }
}

This code contains 2 constants, targetXValue and accel. Because this command extends AutomaticTurnCommand, only setSpeed() needs to be changed in order to drive up to the target.

The robot moves at the constant speed AUTOMATIC_DRIVE_SPEED, but slows down if it is at a steep angle to the target.

A proportion accel is created that decreases as the X offset increases. accel is multiplied by AUTOMATIC_DRIVE_SPEED to slow down the drivetrain to whatever is most accurate. It is then sent to the robot.

Current Constants and What They Do:

1. SPEED_WHILE_TURNING

   • This constant determines the speed that the robot will turn while it is at a steep angle. At the steepest angle, 1, the robot will not move. At the constant of 2, the robot will move at 1/2 speed. At 3, it will move at 2/3 speed, and so on.

   • How To Tune: Approach the target at a slanted angle. If the robot drives so that the target is out of view or the robot does not turn enough, decrease this. CAUTION: Do not set this value to lower than 1.

   • Recommended Value: 1 to 3

2. AUTOMATIC_DRIVE_SPEED

   • This is the maximum speed at which the robot can move during AutomaticDrive.

   • How To Tune Determine if the robot moves too fast or too slow during AutomaticDrive. It may be moving too fast if it can not reach the target.

   • Recommended Value: 0.25 to 0.4 (Depends on Drivetrain)