Running Your Code

Where our commands meet our subsystems

Starting Points

From the little Android driver station, here's where we select our autonomous or teleop opModes. Our autonomous entry point can have some options to indicate where the robot is starting on the field and any decisions about scoring that need to be made.

@Autonomous(name = "Autonomous - Primary")
public class AutoMcAutty extends CommandOpMode {
    /**
     * Set up robot such that it asks the player what our starting position is and kicks off
     * a FTCLib-style RoadRunner.
     */
    @Override
    public void initialize() {
        // QUERY USER TO DETERMINE OUR STARTING COORDINATES
        boolean isLeft = false;
        boolean isRed = false;
        boolean goForBoard = false;
        boolean gotoOpposite = false;
        // give player time to enter selection
        while(opModeInInit()) {
            // press X for blue and B for red
            if (gamepad1.x)
                isRed = false;
            else if (gamepad1.b && !gamepad1.start)
                isRed = true;
            // press dpad LEFT for left and RIGHT for right
            if (gamepad1.dpad_left)
                isLeft = true;
            else if (gamepad1.dpad_right)
                isLeft = false;
            // press Y to go for 45 and A just to park
            if (gamepad1.y)
                goForBoard = true;
            else if (gamepad1.a && !gamepad1.start)
                goForBoard = false;
            // DISPLAY SELECTION
            telemetry.addData("Position", "%s Team, %s Side", isRed ? "Red" : "Blue",
                    isLeft ? "Left" : "Right");
            telemetry.addData("Target Points", "%s", goForBoard ? "45" : "25");
            telemetry.update();
        }
        /*
         We build our robot. From here on out, we don't need this file. When we build the robot,
         all of our buttons are bound to commands and this class's parent, CommandOpMode, will
         continuously run any scheduled commands. We now slide into the WPILib style.
         We pass in our autonomous config variables, which signals to the robot we want to be in
         autonomous mode instead of in teleop mode, which would take no params besides this.
         */
        Robot m_robot = new MyRobot(this, isRed, isLeft, goForBoard);
    }

The teleop kick-off is much simpler:

@TeleOp(name="TeleOp - Primary")
public class DriveyMcDriverson extends CommandOpMode {

    /**
     * Autonomous is over. It's time to drive. Forget RoadRunner's need to track our position
     */
    @Override
    public void initialize() {
        /*
         We build our robot. From here on out, we don't need this file. When we build the robot,
         all of our buttons are bound to commands and this class's parent, CommandOpMode, will
         continuously run any scheduled commands. We now slide into the WPILib style.
         */
        Robot m_robot = new MyRobot(this);
    }

}

These two opModes go to the same place but get there using different constructors. The robot knows whether to run autonomous or teleop based on which constructor is being called.

MyRobot

This is where Commands get lined up for autonomous mode or bound to controller inputs in teleop. MyRobot is a soulless filename; typically, we'll rename this file each year after our robot.

public class MyRobot extends Robot {

    // INSTANCE VARIABLES
    public LinearOpMode opMode;
    public GamepadEx player1;
    public GamepadEx player2;
    public enum AprilTagToAlign {
        LEFT, CENTER, RIGHT, NONE
    }
    public AprilTagToAlign targetApril = AprilTagToAlign.NONE;

    // SUBSYSTEMS
    public MecanumDrive drive;
    public Arm arm;

    /**
     * Welcome to the Command pattern. Here we assemble the robot and kick-off the command
     * @param opMode The selected operation mode
     */
    public MyRobot(LinearOpMode opMode) {
        this.opMode = opMode;
        player1 = new GamepadEx(opMode.gamepad1);
        player2 = new GamepadEx(opMode.gamepad2);
        initTele();
    }
    // OVERLOADED CONSTRUCTOR THAT RESPONDS TO AUTONOMOUS OPMODE USER QUERY
    public MyRobot(LinearOpMode opMode, boolean isRed, boolean isLeft, boolean goForBoard) {
        this.opMode = opMode;
        initAuto(isRed, isLeft, goForBoard);
    }

    /**
     * Set teleOp's default commands and player control bindings
     */
    public void initTele() {
        // throw-away pose because we're not localizing anymore
        drive = new MecanumDrive(this, new Pose2d(0,0,0));
        register(drive);
        drive.setDefaultCommand(new DriveCommand(this));
        // start arm
        arm = new Arm(this);
        register(arm);

        /*
                .__                                      ____
        ______  |  |  _____   ___.__.  ____ _______     /_   |
        \____ \ |  |  \__  \ <   |  |_/ __ \\_  __ \     |   |
        |  |_> >|  |__ / __ \_\___  |\  ___/ |  | \/     |   |
        |   __/ |____/(____  // ____| \___  >|__|        |___|
        |__|               \/ \/          \/
        */
        Button aButtonP1 = new GamepadButton(player1, GamepadKeys.Button.A);
        aButtonP1.whenPressed(new InstantCommand(() -> {
            drive.toggleFieldCentric();
        }));

        Button bButtonP1 = new GamepadButton(player1, GamepadKeys.Button.B);
        bButtonP1.whenPressed(new InstantCommand(() -> {
            drive.resetFieldCentricTarget();
        }));

        Button xButtonP1 = new GamepadButton(player1, GamepadKeys.Button.X);
        Button yButtonP1 = new GamepadButton(player1, GamepadKeys.Button.Y);
        Button dPadUpP1 = new GamepadButton(player1, GamepadKeys.Button.DPAD_UP);
        Button dPadDownP1 = new GamepadButton(player1, GamepadKeys.Button.DPAD_DOWN);
        Button dPadLeftP1 = new GamepadButton(player1, GamepadKeys.Button.DPAD_LEFT);
        Button dPadRightP1 = new GamepadButton(player1, GamepadKeys.Button.DPAD_RIGHT);

        /*
                _                                    __
               (_ )                                /'__`\
         _ _    | |    _ _  _   _    __   _ __    (_)  ) )
        ( '_`\  | |  /'_` )( ) ( ) /'__`\( '__)      /' /
        | (_) ) | | ( (_| || (_) |(  ___/| |       /' /( )
        | ,__/'(___)`\__,_)`\__, |`\____)(_)      (_____/'
        | |                ( )_| |
        (_)                `\___/'
         */
        Button aButtonP2 = new GamepadButton(player2, GamepadKeys.Button.A);
        aButtonP2.whenPressed(new InstantCommand(() -> {
            arm.travelMode();
        }));

        Button bButtonP2 = new GamepadButton(player2, GamepadKeys.Button.B);
        bButtonP2.whenPressed(new InstantCommand(() -> {
            arm.toggleOpen();
        }));

        Button xButtonP2 = new GamepadButton(player2, GamepadKeys.Button.X);
        xButtonP2.whenHeld(new ArmMoveTo(this, ArmMoveTo.ArmPosition.GROUND));

        Button yButtonP2 = new GamepadButton(player2, GamepadKeys.Button.Y);
        yButtonP2.whenHeld(new ArmMoveTo(this, ArmMoveTo.ArmPosition.HIGH_DROP));

        Button dPadUpP2 = new GamepadButton(player2, GamepadKeys.Button.DPAD_UP);
        dPadUpP2.whileHeld(new InstantCommand(() -> {
            arm.wristUp();
        }));

        Button dPadDownP2 = new GamepadButton(player2, GamepadKeys.Button.DPAD_DOWN);
        dPadDownP2.whileHeld(new InstantCommand(() -> {
            arm.wristDown();
        }));

        Button dPadLeftP2 = new GamepadButton(player2, GamepadKeys.Button.DPAD_LEFT);
        dPadLeftP2.whileHeld(new InstantCommand(() -> {
            arm.rollNegative();
        }));

        Button dPadRightP2 = new GamepadButton(player2, GamepadKeys.Button.DPAD_RIGHT);
        dPadRightP2.whileHeld(new InstantCommand(() -> {
            arm.rollPositive();
        }));

    }

    /**
     * Used given starting position and call the corresponding commands
     */
    public void initAuto(boolean isRed, boolean isLeft, boolean goForBoard) {
        // TODO: Calculate pose for each of the four starting positions
        Pose2d start;
        // RED LEFT
        if(isRed && isLeft)
            start = new Pose2d(new Vector2d(0, 0), 0.0);
        // RED RIGHT
        else if(isRed)
            start = new Pose2d(new Vector2d(0, 0), 0.0);
        // BLUE LEFT
        else if(isLeft)
            start = new Pose2d(new Vector2d(0, 0), 0.0);
        // BLUE RIGHT
        else
            start = new Pose2d(new Vector2d(0, 0), 0.0);

        drive = new MecanumDrive(this, start);
        register(drive);
        arm = new Arm(this);
        register(arm);
        

        // locate prop, drop piece, withdraw and straighten out
        new SequentialCommandGroup(
                new ScanForProp(this,0,20, 4),
                new TurnToProp(this, 1),
                new InstantCommand(() -> {
                    arm.openClaw();
                    opMode.sleep(25);
                    arm.travelMode();
                }),
                new StrafeByDistance(this, 0, -2, 0.5),
                new RotateToZero(this, 2)
        ).schedule();

        // TODO: complete autonomous command sequence

    }
}

Two Constructors

• One for teleop, requiring only the opMode object.

• Another for autonomous, taking additional parameters so we know our starting position and what script to run (isRed, isLeft, goForBoard)

initTele

Default Command

Once our drive train drive is registered, we give it a default command, DriveCommand

• defaultCommand is a special command assigned to a subsystem that continuously runs unless interrupted by another higher-priority command.

• The DriveCommand will constantly watch joystick inputs as well as apply dead zones and field-orientated drive

• If you press a button that triggers another command for the drive system (e.g., rotating), that command will interrupt the DriveCommand temporarily until it finishes.

• Once the other command finishes, the DriveCommand automatically resumes as the defaultCommand, listening to joystick inputs again.

Keybindings

The MyRobot code demonstrates different ways to connect gamepad buttons with robot actions using FTCLib. Here's a breakdown of the standard methods and how they translate to gameplay scenarios:

1. whenPressed:

• Definition: Executes a command once when the button is initially pressed.

• Example: Pressing the A button on Gamepad 1 toggles the driving mode.

• FTC Gameplay:

  • Use this for actions that only need to happen once per button press, like activating a boost ability or triggering a quick arm movement.

2. whenReleased:

• Definition: Executes a command once when the button is released.

• Example: Releasing the B button on Gamepad 2 closes the arm claw.

• FTC Gameplay:

  • This is useful for actions that occur when you stop pressing the button, like stopping arm movement or ending a special ability.

3. whileHeld:

• Definition: Continuously executes a command while the button is held down.

• Example: Holding the d-pad on Gamepad 2 raises or lowers the arm's wrist.

4. toggleWhenActive:

• Definition: Starts/stops a command each time the button is pressed, regardless of previous state.

• Example: Not used in the provided code, but it could be used for toggling a light or other on/off functionality.

• FTC Gameplay:

  • This is helpful for quickly activating and deactivating abilities without keeping the button held down.

Here's an example of how to link a button with a command using whileHeld:

Button dPadUpButton = new GamepadButton(gamepad2, GamepadKeys.Button.DPAD_UP);
dPadUpButton.whileHeld(new InstantCommand(() -> {
    arm.wristUp();
}));

This code creates a button listener for the up d-pad on Gamepad 2. When held down, it continuously executes a command that raises the arm's wrist.

initAuto

In MyRobot's initAuto function, the autonomous script takes shape. But how do you structure your commands? Let's delve into the options:

1. Sequential Commands:

• Commands execute one after another, completing before the next starts.

• Think of it as a step-by-step recipe: scan for the prop, turn towards it, open the claw, move closer, etc.

• Benefits:

  • Easy to understand and troubleshoot.

  • Ensures each action finishes cleanly before proceeding.

• Drawbacks:

  • Might be slower if some actions could happen simultaneously.

  • Less flexibility for complex maneuvers involving interactions or timing dependencies.

2. Concurrent Commands:

• Multiple commands run simultaneously, sharing resources and potentially interacting dynamically.

• Imagine multitasking: turning while moving closer or raising the wrist while opening the claw.

• Benefits:

  • Can be more efficient if actions don't rely on each other's completion.

  • Offers greater flexibility for intricate robot behaviors.

• Drawbacks:

  • Requires careful planning to avoid conflicts and ensure smooth coordination.

  • Debugging can be more challenging.

Writing the Script:

initAuto provides flexibility:

• Within initAuto: You can build your entire autonomous sequence directly in our robot file using a new SequentialCommandGroup() with nested commands. This approach keeps everything centralized and concise.

• Separate File: Files are kept shorter if we keep the autonomous procedure in its own SequentialCommandGroup file. This means a programmer tasked with this job could push changes to our GitHub repo with less concern for conflicts. 🤝