Package yams.core.math

package yams.core.math
Yet Another Mechanism System math classes for easier calculations.

This package contains control-theory math utilities used internally by YAMS mechanisms and available for direct use by teams that need low-level control primitives. All classes are designed to integrate seamlessly with WPILib 2026 unit types and the rest of the YAMS API.

Class Overview

  • LQRController and LQRConfig — implement discrete Linear Quadratic Regulator (LQR) control. LQRConfig holds the Q (state cost) and R (input cost) weighting matrices along with the plant model; LQRController solves the discrete algebraic Riccati equation at construction time and exposes a calculate() method suitable for calling from a periodic loop.
  • DerivativeTimeFilter computes a filtered derivative estimate for use as the D term in PID controllers. Rather than a raw finite difference (which amplifies sensor noise), this class applies a configurable low-pass filter to the derivative signal, trading off noise rejection against phase lag.
  • SmartMath a collection of static helper methods for common control-theory and unit-conversion tasks (clamping, interpolation, wrapping angles, etc.) that do not belong to a specific controller class.
  • ExponentialProfilePIDController a profiled PID variant that uses an exponential motion profile to generate setpoints. Compared to a trapezoidal profile, the exponential profile produces smoother acceleration transitions and is less prone to overshoot on mechanisms with significant inertia.

Example LQR Setup

// Build a config for a single-state, single-input system (e.g., flywheel velocity).
LQRConfig config = new LQRConfig()
    .withQWeights(1.0)   // state cost: penalise velocity error
    .withRWeights(12.0)  // input cost: penalise voltage
    .withPlant(plant);   // LinearSystem from SysId or known constants

LQRController lqr = new LQRController(config);

// In periodic():
double voltage = lqr.calculate(currentVelocity, goalVelocity);
motor.setVoltage(voltage);

Example Filtered Derivative

DerivativeTimeFilter dFilter = new DerivativeTimeFilter(0.02 /* dt seconds *\/);

// In periodic():
double filteredDeriv = dFilter.calculate(measurement);
See Also: