Skip to content

Creating OpModes

Lucas Bubner edited this page Mar 19, 2025 · 47 revisions

Important

Be sure you have a RobotConfig set up! You may read this page to learn how to make such a file. You will also want to be familiar with the Paradigms and Subsystems used in these examples.

The most vital component of a BunyipsLib OpMode is the abstract OpMode variant that is chosen. Various OpMode abstractions exist for different purposes, but all pool towards the same ideas. Instead of extending a LinearOpMode, you choose from a variant that suits your application the best. Below is a table of the three OpModes bases used in competition.

Abstract OpMode variant Extends Purpose Use cases
BunyipsOpMode LinearOpMode All-use utility-driven OpMode Any, primarily procedural TeleOp
AutonomousBunyipsOpMode BunyipsOpMode Autonomous with a built-in task queue system Autonomous OpModes
CommandBasedBunyipsOpMode BunyipsOpMode Integrated with the Scheduler, allows built-in command-based paradigms TeleOp w/ commands

There also exists some utility OpModes that can be used but are not designed for competition.

OpMode variant Abstract? Purpose Use cases
HardwareTester No Scans the HardwareMap to provide controls on most hardware objects without needing to make a new OpMode. Supports dashboard controls for motor powers and servo positions. Rapid debugging of new hardware without a TeleOp
RoadRunnerTuningOpMode Yes Uses a telemetry menu to select from the various RoadRunner v1.0 tuning OpModes RoadRunner tuning from one OpMode only
ColourTunerOpMode Yes Live dashboard tuning of ColourThreshold vision processors Debugging colour thresholds using a gamepad
ResetEncoders No Software resets all motor encoders on the robot back to 0 Manual homing of all arms pre-OpMode
ResetLastKnowns No Clears the Storage.memory().lastKnownPosition field Clearing any transient state of last known position

Creating a simple TeleOp

To demonstrate the connection between subsystems and OpMode interactions, a simple TeleOp will be created based on the example hardware-only robot configuration on the configuration page. There are a few different ways of creating a TeleOp with BunyipsLib, primarily Iterative and Command-based TeleOp. Information labelling the technical differences and changes to utilities between these paradigms is located in the Scheduling paradigm section.

Note

These examples use a RobotConfig that uses the hardware-only approach. By using a subsystem approach, you don't have to declare any subsystems at the OpMode level, and you simply have to call robot.subsystem to access these subsystems after the config has been initialised. A hardware-only approach is used here to highlight subsystem interaction.

Iterative TeleOp

The iterative TeleOp is the simplest form and is fairly self-explanatory. Most input/output reactionary code is handled with minimal levels of abstraction.

import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import au.edu.sa.mbhs.studentrobotics.bunyipslib.BunyipsOpMode;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.subsystems.DualServos;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.subsystems.HoldableActuator;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.subsystems.drive.SimpleMecanumDrive;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.Controls;

/**
 * A basic TeleOp that doesn't use command-based functions.
 * This is modelled after the basic robot from the robot configuration page.
 */
@TeleOp
public class BasicTeleOp extends BunyipsOpMode {
    private final Robot config = new Robot();

    // All subsystems must be instantiated during onInit(), otherwise the HardwareMap will be null and the robot will not be happy with you as hardware references are not loaded yet
    private SimpleMecanumDrive drive;
    private HoldableActuator arm;
    private DualServos claws;

    @Override
    protected void onInit() {
        // Required to be called to assign your hardware from your config. This will run onRuntime().
        config.init();

        // For this example, we are continuing to use the SimpleMecanumDrive. More information about which drive subsystem to use in conjunction with using RoadRunner and BunyipsLib is in the Subsystems/RoadRunner sections of this wiki.
        // SimpleMecanumDrive simply runs the Mecanum calculations for a set of four wheels, and is useful for simple implementations.
        // Review the paradigm for robot poses/vectors in the Paradigms section, as all drive subsystems will use this
        drive = new SimpleMecanumDrive(config.front_left, config.back_left, config.back_right, config.front_right); // Wheel configuration parameters are always anti-clockwise starting from the front left motor

        // HoldableActuator is a generic motor-powered component that will hold its position with an encoder when no power is supplied
        arm = new HoldableActuator(config.arm)
                .withName("Actuator"); // All BunyipsLib subsystems expose `with` methods for customisation. Review them!
                                       // `with` methods are where you will pass in hardware for limit switches, control settings, etc...
                                       // These method calls are omitted here, but review the API docs of subsystems to see what's available

        // Generic dual servo controller subsystem. Can take in 4 extra arguments to customise open/close positions, but it is recommended
        // to configure this from a hardware standpoint with scaleRange() and direction
        claws = new DualServos(config.left_claw, config.right_claw);
    }

    @Override
    protected void activeLoop() {
        // Convenience method here will make a new Robot Coordinate System vector based on the Cartesian inputs of the gamepad.
        // This will rotate it 90 degrees to match up, and negate the y component as gamepad y-input is always inverted for whatever reason.
        // Note BunyipsLib provides alias for gamepad properties left_stick_x->lsx left_stick_y->lsy and right_stick_x->rsx for convenience
        drive.setPower(Controls.vel(gamepad1.lsx, gamepad1.lsy, gamepad1.rsx));

        // These methods are exposed as per the generic HoldableActuator subsystem 
        arm.setPower(-gamepad2.lsy);  // Sets the arm to be controlled with the second gamepad's left stick, reversed.
        
        // getDebounced custom method on controllers removes the need to create your own rising-edge detection with booleans
        // This method will do rising edge detection on a selected button, in this case gamepad2.x and gamepad2.b
        if (gamepad2.getDebounced(Controls.X)) {
            claws.toggle(DualServos.ServoSide.LEFT);
        } else if (gamepad2.getDebounced(Controls.B)) {
            claws.toggle(DualServos.ServoSide.RIGHT);
        }

        // Some example telemetry can be added too. All telemetry is automatically piped through FtcDashboard and uses a custom format string.
        // The Utilities section explains this in more detail.
        // Uses .big() for HTML formatting. All telemetry objects made through .add() return a HtmlItem.
        telemetry.add("Left Claw: %", claws.isOpen(DualServos.ServoSide.LEFT) ? "Open" : "Closed").big();
        telemetry.add("Right Claw: %", claws.isOpen(DualServos.ServoSide.RIGHT) ? "Open" : "Closed").big();

        // These send stateful updates to the hardware and are required when working with subsystems yourself.
        // *NO* hardware state should be modified if update is not called.
        // You can also call BunyipsSubsystem.updateAll(); to do this in one line
        drive.update();
        arm.update();
        claws.update();
    }
}

Command-based TeleOp

Subsystems are able to operate based on the BunyipsLib Task system. It is wise to familiarise yourself with the paradigm before using it, as it is powerful but needs you to think differently about robot operation. You have already used subsystems in the previous example, but the command-based implementation expands this to the complete paradigm as explained in the Paradigms section.

Tip

The Task paradigm is strongly recommended to use as it allows you to perform complex driver automation with ease! Tasks are also doubly used in the operation of Autonomous OpModes. While simple OpModes are easier to model against an iterative paradigm, more complex OpModes and ones with default behaviours benefit from a command-based paradigm.

Below is a TeleOp implementing the same functionality as the iterative TeleOp. Note that we extend CommandBasedBunyipsOpMode, and update cycles are managed by an external Scheduler. The intrinsics of these paradigms and OpMode variants are explained separately in the Scheduling paradigm section.

Using the hardware-only RobotConfig example:

import au.edu.sa.mbhs.studentrobotics.bunyipslib.CommandBasedBunyipsOpMode;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.subsystems.DualServos;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.subsystems.HoldableActuator;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.subsystems.drive.SimpleMecanumDrive;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.tasks.HolonomicDriveTask;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.Controls;

/**
 * Basic Command-based TeleOp.
 * This is modelled after the basic robot from the robot configuration page.
 */
public class CommandBasedTeleOp extends CommandBasedBunyipsOpMode {
    private final Robot config = new Robot();

    private SimpleMecanumDrive drive;
    private HoldableActuator arm;
    private DualServos claws;

    @Override
    protected void onInitialise() {
        config.init();

        // Same instantiation of subsystems from the iterative approach
        drive = new SimpleMecanumDrive(config.front_left, config.back_left, config.back_right, config.front_right);
        arm = new HoldableActuator(config.arm);
        claws = new DualServos(config.left_claw, config.right_claw);
    }

    @Override
    protected void assignCommands() {
        // Command-based OpModes uses tasks. Most functions, unless they're explicitly tasks, have task and non-task variants,
        // allowing for use in both kinds of TeleOp. Tasks are accessed through the `tasks` field, which is available on all common subsystems.
        operator().whenPressed(Controls.X)
                .run(claws.tasks.toggleLeft());
        operator().whenPressed(Controls.B)
                .run(claws.tasks.toggleRight());

        // Defines default tasks that will run on the appropriate subsystem when no other task needs to run on this subsystem.
        arm.tasks.control(() -> -gamepad2.lsy).setAsDefaultTask();
        // ... which is syntactic sugar for the following: arm.setDefaultTask(arm.tasks.control(() -> -gamepad2.lsy));
        new HolonomicDriveTask(gamepad1, drive).setAsDefaultTask();
    }

    // periodic() is similar to the activeLoop(), allowing you to integrate any other constant loop code as well without being limited
    // by the abstraction of CommandBasedBunyipsOpMode.
    @Override
    protected void periodic() {
        telemetry.add("Left Claw: %", claws.isOpen(DualServos.ServoSide.LEFT) ? "Open" : "Closed").big();
        telemetry.add("Right Claw: %", claws.isOpen(DualServos.ServoSide.RIGHT) ? "Open" : "Closed").big();
    }
}

Standard TeleOp

This example is an extract from an INTO THE DEEP robot. It uses a command-based paradigm and subsystem definition at the RobotConfig, following the best ways to construct an OpMode for TeleOp.

package org.firstinspires.ftc.teamcode.teleop;

import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import au.edu.sa.mbhs.studentrobotics.bunyipslib.CommandBasedBunyipsOpMode;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.executables.UserSelection;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.UnaryFunction;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.tasks.HolonomicVectorDriveTask;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.tasks.bases.Task;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.Controls;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.util.Threads;

import org.firstinspires.ftc.teamcode.Vance;
import org.firstinspires.ftc.teamcode.tasks.TransferSample;

/**
 * TeleOp for Vance.
 *
 * @author Lachlan Paul, 2024
 */
@TeleOp(name = "TeleOp")
@Config
public class VanceTeleOp extends CommandBasedBunyipsOpMode {
    /**
     * Field-centric mode.
     */
    public static boolean FC = true;
    private final Vance robot = new Vance();

    @Override
    protected void onInitialise() {
        robot.init();

        // Allow the user to select whether they want Field Centric at init (default yes if the selection fails where m is null)
        UserSelection<String> sel = new UserSelection<>(
                 m -> FC = m == null || m.equals("FIELD-CENTRIC"), // callback
                 "FIELD-CENTRIC", // option 1
                 "ROBOT-CENTRIC" // option 2
        );
        // Begin the selection asynchronously
        Threads.start("sel", sel);
        // Let BunyipsOpMode know about the thread status with an init-task
        setInitTask(Task.task().isFinished(() -> !Threads.isRunning("sel"))); // Can also use the init branch here to start the thread
        // setInitTask(Task.waitFor(() -> !Threads.isRunning("sel")); // Alternatively
        
        // Analog sticks will be interpreted with the function f(x)=x^2*sgn(x)
        gamepad1.set(Controls.AnalogGroup.STICKS, UnaryFunction.SQUARE_KEEP_SIGN);
    }

    @Override
    protected void assignCommands() {
        operator().whenPressed(Controls.X)
                .run(robot.claws.tasks.toggleBoth());
        operator().whenPressed(Controls.Y)
                .run(robot.clawRotator.tasks.toggle());
        operator().whenPressed(Controls.B)
                .run(robot.basketRotator.tasks.toggle());
        operator().whenRising(Controls.Analog.RIGHT_TRIGGER, (v) -> v == 1.0)
                .run(robot.verticalLift.tasks.home());

        // TransferSample is a custom Task that performs a SequentialTaskGroup of other Tasks
        operator().whenPressed(Controls.RIGHT_BUMPER)
                .run(new TransferSample(robot.verticalLift, robot.horizontalLift, robot.clawRotator, robot.basketRotator, robot.claws, true));

        HolonomicVectorDriveTask hvdt = new HolonomicVectorDriveTask(gamepad1, robot.drive).withFieldCentric(() -> FC));
        robot.drive.setDefaultTask(hvdt); // alternatively hvdt.setAsDefaultTask();
        driver().whenPressed(Controls.A)
                .run("Reset FC Offset", () -> hvdt.resetFieldCentricOrigin(robot.drive.getPose()));

        robot.verticalLift.tasks.control(() -> -gamepad2.rsy).setAsDefaultTask();
        robot.horizontalLift.tasks.control(() -> -gamepad2.lsy).setAsDefaultTask();
        // remember: these are syntactic sugar methods for robot.verticalLift.setDefaultTask(robot.verticalLift.tasks.control(() -> -gamepad2.rsy)); and vice versa.
        // using setAsDefaultTask() helps avoid double referencing but is only available sometimes where the task has called `on` for you internally
    }
}

Courtesy of Lachlan Paul CSS (Robot "Vance", FTC 22407)

Creating Autonomous

To create Autonomous OpModes, the AutonomousBunyipsOpMode instance is used. This provides the Task infrastructure (like the ones used from the Command-Based TeleOp) to execute a queue of tasks sequentially. The key methods to know how to use in AutonomousBunyipsOpMode are:

Method Description
add(Task) Adds a task to the queue.
addAtIndex(Int, Task) Adds a task at a specific index (0-indexed). Supports Runnable instances.
run(Runnable) Constructs a Lambda to add to the queue. Supports an optional second parameter for task name.
defer(Supplier<Task>) Constructs a DynamicTask to add to the queue.
deferAtIndex(Int, Supplier<Task>) Same as addAtIndex but with defer.
wait(Double, Time) Constructs a WaitTask to add to the queue.
addFirst(Task) Adds a task to the very head of the queue.
addLast(Task) Adds a task to the very tail of the queue.
removeAtIndex(Int) Removes a task at a specific index.
remove(Task) Removes a specific task instance from the queue.
Other variants Additional methods such as deferLast, etc., also exist.

Review the API documentation for more method references.

Simple tasks in Auto

This example will use a HoldableActuator and SimpleMecanumDrive (from an external example RobotConfig) to demonstrate how task systems operate together. RoadRunner and the standard MecanumDrive class are recommended to use for drivebase operations, as shown in the RoadRunner example in the last section.

import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Second;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Seconds;

import androidx.annotation.NonNull;
import androidx.annotation.Nullable;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import au.edu.sa.mbhs.studentrobotics.bunyipslib.AutonomousBunyipsOpMode;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.tasks.WaitTask;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.util.Geometry;
import dev.frozenmilk.util.cell.RefCell;

@Autonomous(name = "Simple Auto")
public class SimpleAuto extends AutonomousBunyipsOpMode {
    private final Robot robot = new Robot();

    @Override
    protected void onInitialise() {
        // Standard initialisation
        robot.init();
    }

    @Override
    protected void onReady(@Nullable RefCell<?> selectedOpMode) {
        // Since we are not using setOpModes, the above parameters can be ignored

        // Tasks should be added here, for example, this routine drives forward for 1 second
        // Time drive is not advised for competition, but is useful for demonstration purposes
        // 1. Queue a Lambda to set the power of the drive to 1
        run(() -> robot.simpleDrive.setPower(Geometry.vel(1, 0, 0))); // setPower calls are non-blocking
        // 2. Queue a WaitTask to wait for 1 second
        wait(1, Second);
        // 3. Queue a Lambda to set the power of the drive to 0
        run(() -> robot.simpleDrive.setPower(Geometry.zeroVel()));

        // The above code can also be written as a single DynamicTask, which allows you to define your own tasks:
        add(Task.task()
                .init(() -> robot.simpleDrive.setPower(Geometry.vel(1, 0, 0)))
                .onFinish(() -> robot.simpleDrive.setPower(Geometry.zeroVel()))
                .timeout(Seconds.of(1)));

        // More usefully, you can use tasks built-in to BunyipsLib, such as HoldableActuator ones
        add(robot.actuator.tasks.goTo(400)); // Queues a task to go to 400 ticks then proceeds
        add(robot.actuator.tasks.home()); // After going up to 400, home back to 0

        // And as seen in the Paradigms, you can compose any type of task. All you have to do is wrap it in `add`
        // Do note that add() returns the task back to you, but any changes such as composing it then will NOT
        // be reflected in the task queue. This is because add has already fired
        add(robot.actuator.tasks.goTo(400)).timeout(Seconds.of(3)); // This will work since timeouts are not task composure
        add(robot.actuator.tasks.goTo(400)
                .with(new WaitTask(3, Seconds))); // This will work since the WaitTask is composed with the goTo task
        add(robot.actuator.tasks.goTo(400))
                .with(new WaitTask(3, Seconds)); // WARNING! This will not compose since the add has already fired!

        // Be very careful with your use of brackets and chaining, as it can lead to unexpected behaviour.
    }

    @Override
    protected void periodic() {
        // You can define any other standard activeLoop code here, just like CommandBasedBunyipsOpMode
        // This method is optional and can be removed if not needed
    }
}

Caution

Methods such as forAtLeast, until, and any other task composing action will not be added to the queue if you compose it using the return value of add, rather than inside of the parameter. Be careful with your use of brackets!

Dynamic selection

Sometimes, the starting position of the robot needs to be known, or other information only available at runtime needs to be accounted for in your OpModes. These are called OpMode variants, and the conventional way of doing this was by creating a fully separate OpMode with a different name.

However, this approach is highly repetitive and can get confusing for your drive team. Instead, the setOpModes function of AutonomousBunyipsOpMode can assist in dynamic selection of OpModes.

Tip

It is recommended to follow a convention for Autonomous OpModes, being that one OpMode should be named the objective of the OpMode, such as "Place Two Samples", rather than "Auto Left", which describes where the robot is, not what it will do. To describe location, the dynamic selection system should be used, not the other way around.

setOpModes works as follows:

  1. Objects, which can be enum values, strings, or most commonly StartingConfiguration objects are passed to the setOpModes function in init
  2. The user is prompted to select values with gamepad buttons on OpMode initialisation, if any setOpModes calls were made
  3. When the user completes the selections, the onReady function is called with a RefCell to the object they selected (i.e. passed into setOpModes). The selected button can be retrieved through UserSelection.getLastSelectedButtons().
  4. The logic in the onReady function that you will implement handles the different cases for OpModes, including alliance side, position, etc.

This process is simplified but is the general procedure of dynamic OpMode selection.

If setOpModes is not defined, onReady is called instantly after onInitialise and the corresponding RefCell is null.

Under the hood, setOpModes is an easy way of defining your own UserSelection and running it asynchronously, waiting for it to finish in the init-phase. For more information regarding selection chaining and internal nature of UserSelection, see the wiki section for it.

Note

This user selection process does not affect any other tasks that occur during init! It runs completely separately and will not affect the process of your init procedure. onReady is guaranteed to be called as soon as the UserSelection object is finished (whether by button or init completion)

An example using strings is shown below.

import androidx.annotation.NonNull;
import androidx.annotation.Nullable;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import au.edu.sa.mbhs.studentrobotics.bunyipslib.AutonomousBunyipsOpMode;
import dev.frozenmilk.util.cell.RefCell;

@Autonomous(name = "My Cool Auto")
public class MyAuto extends AutonomousBunyipsOpMode {
    @Override
    protected void onInitialise() {
        // Do any other forms of normal initialisation here
        
        // List of options for the user to select from
        setOpModes("Variant 1", "Variant 2", "Variant 3");
    }

    // onReady will be called once after the user has made a selection, or the START button is pressed, whichever comes first
    @Override
    protected void onReady(@Nullable RefCell<?> selectedOpMode) {
        if (selectedOpMode == null) {
            // User did not make a selection and pressed the start button
            // Handle this case here
            return;
        }
        
        // Otherwise, we can cast the selectedOpMode to the correct type
        String res = (String) selectedOpMode.get();
        
        switch (res) {
            case "Variant 1":
                // Do something
                break;
            case "Variant 2":
                // Do something
                break;
            case "Variant 3":
                // Do something
                break;
        }
    }
}

The StartingConfiguration class can also be used, as UserSelection supports any type of object. This class provides more information than a simple string, and can hold information such as Alliance, Initial Pose, and any other additional user flags. An example is provided below.

import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Degrees;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Inches;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Meters;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Millimeters;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.StartingConfiguration.blueLeft;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.StartingConfiguration.redRight;

import androidx.annotation.NonNull;
import androidx.annotation.Nullable;

import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import java.util.Set;

import au.edu.sa.mbhs.studentrobotics.bunyipslib.AutonomousBunyipsOpMode;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Angle;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Distance;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Measure;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.StartingConfiguration;
import dev.frozenmilk.util.cell.RefCell;

@Autonomous(name = "My Cool Auto With Cool Position Variants")
public class MyAuto extends AutonomousBunyipsOpMode {
    @Override
    protected void onInitialise() {
        // Do any other forms of normal initialisation here

        // Use the StartingConfiguration logic to "build" a starting location for the robot
        setOpModes(
                // First argument (blueLeft, etc) is the Origin Wall (which also determines alliance colour)
                // tile() is the tile number, starting at 1 to be in the center of the Blue Left Wall
                //    optionally, the translate() method can be used to define a distance from the Blue Left Wall
                // backward() is how far back the robot is from the center of the tile
                // rotate() is the +counter-clockwise rotation of the robot from facing out into the field from the wall
                // flag() can be used to add any extra information to the selection
                blueLeft().tile(2).backward(Inches.of(2)).rotate(Degrees.of(90)).flag("Park"),
                // repeat for other starting locations
                redRight().translate(Meters.of(2)).backward(Millimeters.of(40)).rotate(Degrees.of(180))
        );
    }

    @Override
    protected void onReady(@Nullable RefCell<?> selectedOpMode) {
        if (selectedOpMode == null) {
            // Edge case: the user has not selected an opmode
            return;
        }
        
        // Cast to the correct type, which will be StartingConfiguration.Position for these
        StartingConfiguration.Position position = (StartingConfiguration.Position) selectedOpMode.require();
        
        // You now have access to various methods to help construct your Auto, such as:
        StartingConfiguration.Alliance alliance = position.getAlliance(); // the alliance colour
        boolean isBlue = position.isBlue(); // on the blue alliance
        boolean isRed = position.isRed(); // on the red alliance
        
        StartingConfiguration.Origin originWall = position.getOrigin(); // the wall the robot is starting from
        boolean isLeft = position.isLeft(); // origin is left side of field
        boolean isRight = position.isRight(); // origin is right side of field
        
        Measure<Distance> horizontalTranslation = position.getHorizontalTranslation(); // how far from the origin wall
        Measure<Distance> backward = position.getBackwardTranslation(); // how far back from the center of the tile
        Measure<Angle> rotation = position.getCcwRotation(); // how much the robot is rotated from facing out into the field
        // Alternatively, and more usefully:
        Pose2d robotStartingPose = position.toFieldPose(); // for relocalisation with RoadRunner
        
        String informativeString = position.toVerboseString(); // a string that describes the position (normal
                                                               // toString() is also available, it will return a HTML formatted string)
        Set<Object> extraUserFlags = position.getFlags(); // any extra flags the user has set
    }
}

The StartingConfiguration object returns HTML formatted strings on the default toString() method, thus for a OpMode configuration such as:

setOpModes(
    redRight().tile(3.5).backward(Inches.of(1)),
    blueRight().tile(3.5).backward(Inches.of(1))
);

it will be shown up on the Driver Station (in BunyipsLib v6.0.0) as:

RoadRunner-integrated full Auto

Review the RoadRunner page for how to set up and tune a RoadRunner drive to make trajectories.

import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Degrees;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Inches;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Milliseconds;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.Radians;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.StartingConfiguration.blueLeft;
import static au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.StartingConfiguration.redLeft;

import androidx.annotation.NonNull;
import androidx.annotation.Nullable;

import com.acmerobotics.roadrunner.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

import au.edu.sa.mbhs.studentrobotics.bunyipslib.AutonomousBunyipsOpMode;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.tasks.bases.Task;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.tasks.groups.ParallelTaskGroup;
import au.edu.sa.mbhs.studentrobotics.bunyipslib.transforms.StartingConfiguration;
import dev.frozenmilk.util.cell.RefCell;

@Autonomous(name = "Roads Are Being Run")
public class RoadRunnerAuto extends AutonomousBunyipsOpMode {
    private final Robot robot = new Robot();

    @Override
    protected void onInitialise() {
        robot.init();
        setOpModes(
                blueLeft().tile(3.5).backward(Inches.of(2)), // Center of blue wall, touching
                redLeft().tile(3.5).backward(Inches.of(2)) // Center of red wall, touching
        );
    }

    @Override
    protected void onReady(@Nullable RefCell<?> selectedOpMode) {
        if (selectedOpMode == null) return;
        StartingConfiguration.Position position = (StartingConfiguration.Position) selectedOpMode.require();

        // Set the initial pose to the one specified by the user
        robot.drive.setPose(position.toFieldPose());

        // We can now build trajectories!
        robot.drive.makeTrajectory()
                .strafeTo(new Vector2d(24, 24), Inches)
                .addTask(); // util to add the trajectory to the task list directly

        // Or we can build trajectories with more complex paths and other actions to run in parallel
        add(robot.drive.makeTrajectory(new Vector2d(24, 24), Inches, 0, Degrees)
                // Do stuff
                .splineToSplineHeading(new Vector2d(48, 48), Inches, 90, Degrees, 45, Degrees)
                .build()
                // Compose with ParallelTaskGroup and SequentialTaskGroup
                .with(robot.actuator.tasks.goTo(600).then(robot.claws.tasks.openBoth())));

        // Review the RoadRunner section for more information on how to use RoadRunner, and Paradigms
        // to familiarise yourself with task composition.

        // Closing the left claw is a great idea!
        add(robot.claws.tasks.closeLeft());
        // Just kidding, let's not queue that
        removeLast();

        // Servos set instantly, the tasks are instant, let's make a minimum delay so we can place things properly
        add(robot.claws.tasks.closeBoth().forAtLeast(500, Milliseconds));

        // Time to dance (this task is very useless in the real world), unless you have a need for an infinite
        // task that homes the arm in both directions all the time
        Task danceForever = robot.actuator.tasks.ceil().then(robot.actuator.tasks.home()).repeatedly();
        // Get groovy
        add(new ParallelTaskGroup(
                danceForever,
                robot.drive.makeTrajectory()
                        .turn(9000, Radians) // May cause dizziness
                        .build()
        ));
    }
}
Clone this wiki locally