Skip to content

Utilities

Lucas Bubner edited this page Dec 24, 2024 · 32 revisions

BunyipsLib has many utilities to accelerate the developer experience. This article is dedicated to some of the most common utilities in place.

Note

This list of utilities is not exhaustive! There are many more utilities available throughout BunyipsLib that can be best found by searching the codebase or API documentation.

Unit system

The BunyipsLib unit system is modelled after the WPILib Java Units Library (prior to the 2025 rewrite)

The units system is a tool that helps programmers avoid mistakes related to units of measurement. It does this by keeping track of the units of measurement, and by ensuring that all operations are performed with the correct units. This can help to prevent errors that can lead to incorrect results, such as adding a distance in inches to a distance in meters.

An added benefit is readability and maintainability, which also reduces bugs. By making the units of measurement explicit in your code, it becomes easier to read and understand what your code is doing. This can also help to make your code more maintainable, as it is easier to identify and fix errors related to units of measurement.

The units system has a number of features:

  • A set of predefined units, such as meters, degrees, and seconds.
  • The ability to convert between different units.
  • Support for performing arithmetic and comparisons on quantities with units.
  • Support for displaying quantities with units in a human-readable format.

Note

BunyipsLib uses the unit system in various places! It is important to familiarise yourself with the inner workings of the unit system, as they are used frequently when talking about real-world units and interactions.

Terminology

Dimension

Dimensions represent the nature of a physical quantity, such as length, time, or mass. They are independent of any specific unit system. For example, the dimension of meters is length, regardless of whether the length is expressed in meters, millimeters, or inches.

Unit

Units are specific realizations of dimensions. They are the way of expressing physical quantities. Each dimension has a base unit, such as the meter for length, the second for time, the kilogram for mass. Derived units are formed by combining base units, such as meters per second for velocity.

Measure

Measures are the specific magnitude of physical quantities, expressed in a particular unit. For example, 5 meters is a measure of distance.

These concepts are used within the Unit Library. For example, the measure 10 seconds has a magnitude of 10, the dimension is time, and the unit is seconds.

Usage

The Java units system from WPILib is available in the au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units package. The most relevant classes are:

  • The various classes for predefined dimensions, such as Distance and Time
  • Units, which contains a set of predefined units. Take a look a the Units JavaDoc to browse the available units and their types
  • Measure, which is used to tag a value with a unit.

Important

BunyipsLib may use the unit system in several contexts, from taking a Measure instance in directly (func(Inches.of(15))) to supplying two arguments, usually double and a Unit of some particular derived type (func(15, Inches)), internally composing a Measure. Ensure to read the corresponding documentation to know which context is appropriate.

Tip

The static import au.edu.sa.mbhs.studentrobotics.bunyipslib.external.units.Units.* gives full access to all predefined units.

Java Generics

Units of measurement can be complex expressions involving various dimension, such as distance, time, and velocity. Nested generic type parameters allow for the definition of units that can represent such complex expressions. Generics are used to keep the library concise, reusable, and extensible, but it tends to be verbose due to the syntax for Java generics.

For instance, consider the type Measure<Velocity<Distance>>. This type represents a measurement for velocity, where the velocity itself is expressed as a unit of distance per unit of time. This nested structure allows for the representation of units like meters per second or feet per minute. Similarly, the type Measure<Per<Voltage, Velocity<Distance>>> represents a measurement for a ratio of voltage to velocity.

It’s important to note that not all measurements require such complex nested types. For example, the type Measure<Distance> is sufficient for representing simple units like meters or feet. However, for more complex units, the use of nested generic type parameters is essential.

For local variables, you may choose to use Java’s var keyword (if your project is on JDK 10+) instead of including the full type name. For example, these are equivalent:

Measure<Per<Voltage, Velocity<Distance>>> v = VoltsPerMeterPerSecond.of(8);
var v = VoltsPerMeterPerSecond.of(8);

Creating Measures

The Measure class is a generic type that represents a magnitude (physical quantity) with its corresponding unit. It provides a consistent and type-safe way to handle different dimensions of measurements, such as distance, angle, and velocity, but abstracts away the particular unit (e.g. meter vs. inch). To create a Measure object, you call the Unit.of method on the appropriate unit object. For example, to create a Measure<Distance> object representing a distance of 6 inches, you would write:

Measure<Distance> wheelDiameter = Inches.of(6);

Other measures can also be created using their Unit.of method:

Measure<Mass> armMass = Kilograms.of(1.423);
Measure<Distance> armLength = Inches.of(32.25);
Measure<Angle> minArmAngle = Degrees.of(5);
Measure<Angle> armMaxTravel = Rotations.of(0.45);
Measure<Velocity<Distance>> maxSpeed = MetersPerSecond.of(2.5);

Performing Calculations

The Measure class also supports arithmetic operations, such as addition, subtraction, multiplication, and division. These are done by calling methods on the objects. These operations always ensure that the units are compatible before performing the calculation, and they return a new Measure object. For example, you can add two Measure<Distance> objects together, even if they have different units:

Measure<Distance> distance1 = Inches.of(10);
Measure<Distance> distance2 = Meters.of(0.254);

Measure<Distance> totalDistance = distance1.plus(distance2);

Measure<Distance> halfField = FieldTiles.of(3);
Measure<Distance> fullField = halfField.times(2);
fullField.isEquivalent(Feet.of(12)); // true

In this code, the units system will automatically convert the measures to the same unit before adding the two distances. The resulting totalDistance object will be a new Measure<Distance> object that has a value of 0.508 meters, or 20 inches.

This example combines the wheel diameter and gear ratio to calcualate the distance per rotation of the wheel:

Measure<Distance> wheelDiameter = Inches.of(3);
double gearRatio = 10.48;
Measure<Distance> distancePerRotation = wheelDiameter.times(Math.PI).divide(gearRatio);

Important

By default, arithmetic operations create new Measure instances for their results. Do consider the memory impact of these objects with the JVM Garbage Collector if using many, and in memory-limited situations consider reading the Mutability and Object Creation section below.

Converting Units

Unit conversions can be done by calling Measure.in(Unit), or Measure.to(Unit). The Java type system will prevent units from being converted between incompatible types, such as distances to angles. The returned values will be bare double values without unit information - it is up to you, the programmer, to interpret them correctly! It is strongly recommended to only use unit conversions when interacting with APIs that do not support the units library.

Measure<Velocity<Distance>> maxVelocity = FeetPerSecond.of(12.5);
Measure<Velocity<Velocity<Distance>>> maxAcceleration = FeetPerSecond.per(Second).of(22.9);

double maxVelM = maxVelocity.in(MetersPerSecond); // => OK! Returns 3.81
maxVelocity.in(RadiansPerSecond); // => Compile error! Velocity<Angle> cannot be converted to Unit<Velocity<Distance>>

// Application of unit converstion to SI units for a TrapezoidalProfile
TrapezoidProfile.Constraints driveConstraints = new TrapezoidProfile.Constraints(
    maxVelocity.in(MetersPerSecond),
    maxAcceleration.in(MetersPerSecondPerSecond)
);

Human-readable Formatting

The Measure class has methods that can be used to get a human-readable representation of the measure. This feature is useful to display a measure on telemetry or in logs.

  • toScientificString() returns a string representation of the measure in a shorthand scientific form. The symbol of the backing unit is used, rather than the full name, and the magnitude is represented in scientific notation. For example, 1.234e+04 V/m
  • toShortString() return a string representation of the measure in a shorthand form. The symbol of the backing unit is used, rather than the full name, and the magnitude is represented in a full string, not scientific notation. For example, 1234 V/m
  • toLongString() returns a string representation of the measure in a longhand form. The name of the backing unit is used, rather than its symbol, and the magnitude is represented in a full, like the short string. For example, 1234 Volt per Meter

Tip

By default, the standard toString() call on a Measure will return the toShortString() definition!

Mutability and Object Creation

To reduce the number of object instances you create, and reduce memory usage, a special MutableMeasure class is available. You may want to consider using mutable objects if you are using the units library repeatedly, such as in the robot’s periodic loop.

Important

It is often not necessary to use the MutableMeasure in BunyipsLib!

MutableMeasure allows the internal state of the object to be updated, such as with the results of arithmetic operations, to avoid allocating new objects. Special care needs to be taken when mutating a measure because it will change the value every place that instance is referenced. If the object will be exposed as part of a public method, have that method return a regular Measure in its signature to prevent the caller from modifying your internal state.

Extra methods are available on MutableMeasure for updating the internal value. Note that these methods all begin with the mut_ prefix - this is to make it obvious that these methods will be mutating the object and are potentially unsafe! For the full list of methods and API documentation, see the MutableMeasure API documentation.

Method Functionality
mut_plus(double, Unit) Increments the internal value by an amount in another unit. The internal unit will stay the same.
mut_plus(Measure) Increments the internal value by another measurement. The internal unit will stay the same.
mut_minus(double, Unit) Decrements the internal value by an amount in another unit. The internal unit will stay the same.
mut_minus(Measure) Decrements the internal value by another measurement. The internal unit will stay the same.
mut_times(double) Multiplies the internal value by a scalar.
mut_divide(double) Divides the internal value by a scalar.
mut_replace(double, Unit) Overrides the internal state and sets it to equal the given value and unit.
mut_replace(Measure) Overrides the internal state to make it identical to the given measurement.
mut_setMagnitude(double) Overrides the internal value, keeping the internal unit. Be careful when using this!
MutableMeasure<Distance> measure = MutableMeasure.zero(Feet);
measure.mut_plus(10, Inches);    // 0.8333 feet
measure.mut_plus(Inches.of(10)); // 1.6667 feet
measure.mut_minus(5, Inches);    // 1.25 feet
measure.mut_minus(Inches.of(5)); // 0.8333 feet
measure.mut_times(6);            // 0.8333 * 6 = 5 feet
measure.mut_divide(5);           // 5 / 5 = 1 foot
measure.mut_replace(6.2, Meters) // 6.2 meters - note the unit changed!
measure.mut_replace(Millimeters.of(14.2)) // 14.2mm - the unit changed again!
measure.mut_setMagnitude(72)     // 72mm

Warning

MutableMeasure objects can - by definition - change their values at any time! It is unsafe to keep a stateful reference to them - prefer to extract a value using the Measure.in method, or create a copy with Measure.copy that can be safely stored. For the same reason, authors who choose to use the unit system must also be careful about methods accepting the supertype Measure. Review mitigation strategies on the official docs.

Defining New Units

There are four ways to define a new unit that isn’t already present:

  • Using the Unit.per or Unit.mult methods to create a composite of two other units;
  • Using the Milli, Micro, and Kilo helper methods;
  • Using the derive method and customizing how the new unit relates to the base unit; and
  • Subclassing Unit to define a new dimension.

New units can be defined as combinations of existing units using the Unit.mult and Unit.per methods.

Per<Voltage, Distance> VoltsPerInch = Volts.per(Inch);
Velocity<Mass> KgPerSecond = Kilograms.per(Second);
Mult<Mass, Velocity<Velocity<Distance>> Newtons = Kilograms.mult(MetersPerSecondSquared);

Using mult and per will store the resulting unit. Every call will return the same object to avoid unnecessary allocations and garbage collector pressure.

@Override
protected void activeLoop() {
    // Feet.per(Millisecond) creates a new unit on the first loop,
    // which will be reused on every successive loop
    telemetry.addData("Speed", drivebase.getSpeed().in(Feet.per(Millisecond)));
}

Note

Calling Unit.per(Time) will return a Velocity unit, which is different from and incompatible with a Per unit!

New dimensions can also be created by subclassing Unit and implementing the two constructors. Note that Unit is also a parameterized generic type, where the generic type argument is self-referential; Distance is a Unit<Distance>. This is what allows us to have stronger guarantees in the type system to prevent conversions between unrelated dimensions.

public class ElectricCharge extends Unit<ElectricCharge> {
    public ElectricCharge(double baseUnitEquivalent, String name, String symbol) {
        super(ElectricCharge.class, baseUnitEquivalent, name, symbol);
    }

    // required for derivation with Milli, Kilo, etc.
    public ElectricCharge(UnaryFunction toBaseConverter, UnaryFunction fromBaseConverter, String name, String symbol) {
        super(ElectricCharge.class, toBaseConverter, fromBaseConverter, name, symbol);
    }
}

public static final ElectricCharge Coulomb = new ElectricCharge(1, "Coulomb", "C");
public static final ElectricCharge ElectronCharge = new ElectricCharge(1.60217646e-19, "Electron Charge", "e");
public static final ElectricCharge AmpHour = new ElectricCharge(3600, "Amp Hour", "Ah");
public static final ElectricCharge MilliampHour = Milli(AmpHour);

HTML and Text

The DualTelemetry object used natively in the BunyipsOpMode uses HTML formatting to provide colours, text size, and styling to telemetry messages.

BunyipsLib also uses a custom format string solution rather than the general String.format method, which is essential to know for the various places format strings are natively implemented.

BunyipsLib Format String

String.format requires the use of format specifiers, similar to C. The format string used in BunyipsLib expands upon this concept, but instead uses a singular % to specify where values should be substituted.

Note

The format string used in BunyipsLib is very similar to the built-in MessageFormat. It only requires one placeholder value, and internally calls toString() on given parameters.

String a = Text.format("This is my % string, %, %, %", "cool", 1, 1.2, new Object());
a; // This is my cool string, 1, 1.2, java.lang.Object@XXXXXXXX

Tip

If you want to actually include a percentage symbol (%) in your string, append two \\ before it (e.g. Text.format("%\\%", 100); // 100%)

Internal methods that support a variable argument of extra parameters, such as in telemetry, logging, etc. all use this specification.

Note

If you wish to do any formatting as you normally would through a String.format call, you can consider the Mathf.round function for trimming long floating point values, or use a conventional String.format.

Tip

Text offers a StringBuilder-like class, through Text.builder(), which internally composes Text.format calls on the append method (e.g. Text.builder().append("hello %", "world").build() -> hello world). It is used internally.

HTML

You can construct telemetry-friendly styling using the Text.html() builder (which is effectively a StringBuilder but styling is mentioned instead of the usual append), or you can consult this table to see what common options are available for you, and you can make the HTML tags yourself:

Function Opening Tag
Bold <b>
Italic <i>
Big <big>
Small <small>
Underline <u>
Strikethrough <s>
Superscript <sup>
Subscript <sub>
Header (1-6) <hX>, X from 1-6
Monospace <font face="monospace">
Foreground Color <font color="CSS_COLOUR">
Background Color <span style="background-color:CSS_COLOUR">

Tip

If you're using a DualTelemetry instance, such as the telemetry field of BunyipsOpMode, adding an object to telemetry through add() will return an HtmlItem, which can be used to quickly style the entire message (e.g. telemetry.add("hi").color("red").bold();)

Logcat

BunyipsLib automatically logs and reports errors into Logcat, the Android logger that the SDK also uses to log operations. To access Logcat in Android Studio, connect to the robot, and while the app is connected open the Logcat panel on the left.

BunyipsLib-related logs will always be stored under the BELLOWER tag by default.

To add your own logs to be shown in Logcat, use the Dbg class.

Important

All Dbg messages use the Text format string, as described in the above section.

Dbg.log()

This is the most commonly used log call for user code, which will display logs in Logcat at an INFO level. BunyipsLib internals seldom use Dbg.log(), it is designed for user-code.

Tip

There also exists a purposefully deprecated Dbg.logTmp() method, functioning the same as the Dbg.log() but is deprecated to be caught in static code analysis. This is good for temporary log calls that you don't want to be committing into your codebase.

Dbg.logd()

Used mostly for BunyipsLib internal logs, logd displays logs at a DEBUG level.

Dbg.logv()

logv is used for behaviours that always occur, such as notifying the firing of certain methods in a BunyipsOpMode. It logs at a VERBOSE level.

Dbg.warn()

Warn is prepended by the WRN_PREPEND string, which by default is !

warn is for peculiar program states that should be reported to the user. It is used in internal BunyipsLib for some implementations.

Dbg.error()

Error is prepended by the ERR_PREPEND string, which by default is !!

error is for states where continuing program execution will result in degraded functionality. It is used in internal BunyipsLib for some implementations.

Dbg.stamp()

stamp is an implementation of log that will log an INFO to Logcat with the current user method context (from the Exceptions class), and optionally attempts to get a current BunyipsOpMode instance to extract the MovingAverageTimer elapsedTime() field in seconds. It is useful for a sanity check log.

Tip

All Dbg methods support taking in a class instance or stack trace element that will be auto-prepended to the message, such as Dbg.log(getClass(), "error %", 1); will show up as an INFO: [SimpleClassName] error 1

Catch-all exception handler

Tying hand-in-hand with the Dbg class, a catch-all exception handler is implemented via the Exceptions class.

The exceptions handler is used internally across BunyipsLib, in places such as BunyipsOpMode and derivatives, Task, BunyipsSubsystem, and in any general place where user-code is exposed.

If an exception is caught with this handler, it is swallowed and logged to Logcat via Dbg.error() (and Dbg.sendStackTrace(), an implementation of error), and also logged to an error-out. This error-out for BunyipsLib implementations is the Driver Station telemetry log, which will show up at the very bottom of your telemetry.

Important

Exceptions will NOT catch InterruptedException, ForceStopException, or the BunyipsLib EmergencyStop. If you want to terminate an OpMode immediately, you can throw a new EmergencyStop which Exceptions will not swallow.

There is no setup you need to do to use the catch-all handler using all BunyipsLib utilities. If you'd like to use it manually for finer-grain exception control, you can use one of two ways:

// In the context of a BunyipsOpMode
try {
    someRiskyCode();
} catch (Exception e) {
    Exceptions.handle(e, telemetry::log); // using a DualTelemetry log (default used in BunyipsLib)
}

// More concisely, you can pass single functions as a reference, which will perform the above code the same
Exceptions.runUserMethod(this, this::someRiskyCode); // `this` parameter is a BunyipsOpMode

Memory

Oftentimes, storing data in your OpModes is necessary for operation. However, storing information across different OpModes does need some consideration. Fortunately, there exists a utility class, Storage, which handles both in-memory (from robot power on), and long-term storage (saved to disk).

Tip

Like volatile storage, motor encoders do not reset between OpModes, so the position calls on them will persist until the next power cycle!

There exists a few integrated fields for volatile memory storage, including last known odometry position, last known alliance from a StartingConfiguration selection via UserSelection, a list of all hardware device errors from a RobotConfig, and a general in-memory HashMap for you to use. These can be accessed via the Storage.memory() singleton.

Storage.memory().lastKnownPosition; // auto-updated via the Accumulator
Storage.memory().lastKnownAlliance; // auto-updated via a StartingConfiguration selection on UserSelection
Storage.memory().setVolatile("key", "value");
Storage.memory().getVolatile("key"); // returns "value" as an Object

For long term storage, a GSON-serialised object is saved to disk in the robot data directory, under a file called bunyipslib_storage.json. It can be used to store any seralisable Java object. A try-with-resources (or a try-finally with a close() invocation) should be used to access, modify, and remove values, through the Storage.filesystem() singleton.

try (Storage.Filesystem fs = Storage.filesystem()) { // try-with-resources
    fs.access().put("object", new Object()); // saved to disk after the block
    fs.access().get("object");
}

For static implementations across BunyipsLib, the primary cleanup method Storage.resetAllStaticFieldsForOpMode() is in this class. It is useful if you are using your own OpMode abstractions.

Filters

BunyipsLib offers several filters for fusing and evaluating data. These are integrated as static classes of the overarching Filter class.

Weighted Fusion

The Filter.WeightedFusion takes two sensor inputs and evaluates their average as per a singular gain. This gain handles how much "weight" is distributed between them, such that having a gain set with [0.5, 0.5] trusts both sensors equally, and [1.0, 0.0] trusts the first sensor with full certainty.

DoubleSupplier input1 = () -> 0;
DoubleSupplier input2 = () -> 1;
Filter.WeightedFusion filter = new Filter.WeightedFusion(new DoubleSupplier[]{input1, input2}, new double[]{0.5, 0.5});
filter.getAsDouble(); // returns 0.5

Low Pass Filter

The low-pass filter is used for smoothening out data over time. It may introduce phase lag, however, it is useful in applications such as noisy derivative data - internally used in the Encoder and PIDFController.

A singular gain controls the filter, which is bounded in the domain (0, 1) exclusive. Higher gains towards 1 will smooth and take longer to respond to changes in the source data, whereas lower values towards 0 will react faster but be subject to more noise.


Filter.LowPass filter = new Filter.LowPass(0.8);
filter.apply(processVariable); // processVariable is a double

1D Kalman Filter

The Kalman filter is smarter version of a filter that allows sensor data to be fused by maintaining estimates and state transitions of a particular system.

It consists of two gains, being R, which puts more trust in the model of the system, such a kinematic model that reflects a perfect universe, and Q, which is the sensor reading trust. A well-balanced filter is able to smooth out data and reject disturbance while not introducing any phase lag.


The Kalman filter in BunyipsLib is only implemented for 1-dimension. You can read more about the Kalman filter implemented here.

Filter.Kalman filter = new Filter.Kalman(0.4, 0.3); // 0.4 is R gain, 0.3 is Q gain
filter.calculate(absoluteModel, sensorReading);
// or filter.calculateFromDelta(modelIncrement, sensorReading);

External programs

Several external programs exist that accompany BunyipsLib to accelerate development. These programs are forked to be compatible with BunyipsLib syntax and paradigms, described here in order of usefulness:

From https://github.com/acmerobotics/MeepMeep

A built-in tool integrated directly into BunyipsLib is the MeepMeep Path Visualiser for RoadRunner. It is equipped with the same utilities from BunyipsLib, including the TaskBuilder and unit system. Review the MeepMeepRunner file in the MeepMeep subdirectory of BunyipsLib for more instructions on how to use MeepMeep, exclude it from git, and build visualised trajectories.

Tip

Pressing CTRL+C with MeepMeep open will copy the currently hovered coordinate on the field (in inches) to the clipboard!

From https://github.com/Jarhead20/RRPathGen

An external jar file that can be used for sketching out RoadRunner trajectories is BunyipsLib RRPathGen. Providing BunyipsLib-friendly syntax on generation, auto-export, and being fully compatible with RoadRunner v1.0, this fork allows you to make rough trajectories and visualise points on the field, visualising in the FTC coordinate system (inches and degrees) and exporting to FieldTiles, Inches, or Centimeters with Degrees. This is especially useful for tuning existing waypoints.

This fork can be downloaded from here.

From https://github.com/Beta8397/virtual_robot

Used for sanity tests of BunyipsLib and robot operation, a virtual robot is available through this repository. It is a stripped-down version of BunyipsLib that works with RoadRunner v1.0, allowing you to sanity check OpMode behaviour without needing a robot.

Instructions on how to port over later versions of BunyipsLib are available in this README. This is usually done through the IntelliJ Patch system whenever new changes to main BunyipsLib are made.