TippingLib

TippingLib.java

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import java.util.Optional;

/**
 * {@code AntiTipping} provides a proportional correction system to prevent the robot from tipping
 * over during operation.
 *
 * <p>It uses pitch and roll measurements (in degrees) to detect excessive inclination and computes
 * a correction velocity in the opposite direction of the tilt. The resulting correction can be
 * added to the robot’s translational velocity to help stabilize it.
 *
 * <h2>Usage</h2>
 * <ol>
 *   <li>Instantiate with pitch and roll suppliers and initial configuration parameters.
 *   <li>Call {@link #calculate()} periodically (e.g. once per control loop).
 *   <li>Add the correction from {@link #getVelocityAntiTipping()} to your drive command.
 * </ol>
 *
 * <h2>Configuration</h2>
 * <ul>
 *   <li>{@link #setTippingThreshold(double)} — sets the tipping detection threshold in degrees.
 *   <li>{@link #setMaxCorrectionSpeed(double)} — sets the maximum correction velocity (m/s).
 * </ul>
 *
 * <p>The correction is purely proportional: {@code correction = kP * inclinationMagnitude}, and
 * clamped to {@code maxCorrectionSpeed}.
 * 
 *
 * @since 2025
 */
public class AntiTipping {

  private double tippingThresholdDegrees;
  private double maxCorrectionSpeed; // m/s
  private double kP; // proportional gain

  private double pitchDegrees = 0.0;
  private double rollDegrees = 0.0;
  private double correctionSpeed = 0.0;
  private double inclinationMagnitude = 0.0;
  private double yawDirectionDeg = 0.0;
  
  private Rotation2d tiltDirection = new Rotation2d();

  /**
   * Creates a new {@code AntiTipping} instance.
   *
   * @param kP proportional gain for correction
   * @param tippingThresholdDegrees tipping detection threshold (degrees)
   * @param maxCorrectionSpeed maximum correction velocity (m/s)
   */
  public AntiTipping(
      double kP,
      double tippingThresholdDegrees,
      double maxCorrectionSpeed) {

    this.kP = kP;
    this.tippingThresholdDegrees = tippingThresholdDegrees;
    this.maxCorrectionSpeed = maxCorrectionSpeed;
  }

  /** Gets the tipping detection threshold in degrees. */
  public double getTippingThreshold() {
    return tippingThresholdDegrees;
  }

  /** Sets the tipping detection threshold in degrees. */
  public void setTippingThreshold(double degrees) {
    this.tippingThresholdDegrees = degrees;
  }

  /** Gets the maximum correction velocity in meters per second. */
  public double getMaxCorrectionSpeed() {
    return maxCorrectionSpeed;
  }

  /** Sets the maximum correction velocity in meters per second. */
  public void setMaxCorrectionSpeed(double speedMetersPerSecond) {
    this.maxCorrectionSpeed = speedMetersPerSecond;
  }

  /** Gets the proportional gain for correction. */
  public double getKP() {
    return kP;
  }

  /** Sets the proportional gain for correction. */
  public void setKP(double kP) {
    this.kP = kP;
  }

  /**
   * Updates tipping detection and computes the proportional correction.
   *
   * <p>This method updates internal values (pitch, roll, direction, magnitude, etc.) and generates
   * a correction {@link ChassisSpeeds} vector that can be applied to stabilize the robot.
   * It should be called periodically (e.g. once per control loop).
   * 
   * @param attitude current robot attitude as a {@link Rotation3d}
   * @return correction {@link ChassisSpeeds} to counteract tipping
   */
  public Optional<ChassisSpeeds> calculate(Rotation3d attitude) {
    pitchDegrees = Math.toDegrees(attitude.getY());
    rollDegrees = Math.toDegrees(attitude.getX());

    boolean isTipping =
        Math.abs(pitchDegrees) > tippingThresholdDegrees
            || Math.abs(rollDegrees) > tippingThresholdDegrees;

    // Tilt magnitude (hypotenuse of pitch and roll)
    inclinationMagnitude = Math.hypot(pitchDegrees, rollDegrees);

    if (inclinationMagnitude > 0.0) {
      // Tilt direction (the direction the robot is falling towards) in NWU axes.
      tiltDirection = new Rotation2d(pitchDegrees, rollDegrees);
      yawDirectionDeg = tiltDirection.getDegrees();
    } else {
      tiltDirection = new Rotation2d();
      yawDirectionDeg = 0.0;
    }

    // Proportional correction (opposite the tilt direction)
    correctionSpeed = -kP * inclinationMagnitude;
    correctionSpeed = MathUtil.clamp(correctionSpeed, -maxCorrectionSpeed, maxCorrectionSpeed);

    // Correction vector (field-relative, NWU)
    Translation2d correctionVector =
        inclinationMagnitude > 0.0
            ? new Translation2d(pitchDegrees, rollDegrees)
                .div(inclinationMagnitude)
                .times(correctionSpeed)
            : new Translation2d();

    if (isTipping) {
      return Optional.of(new ChassisSpeeds(correctionVector.getX(), correctionVector.getY(), 0));
    } else {
      return Optional.empty();
    }
  }

  /** Returns the latest tilt magnitude (hypotenuse of pitch and roll). */
  public double getLastInclinationMagnitude() {
    return inclinationMagnitude;
  }

  /** Returns the most recent tilt direction in degrees (pseudo-yaw). */
  public double getLastYawDirectionDeg() {
    return yawDirectionDeg;
  }

  /** Returns the most recent tilt direction as a {@link Rotation2d}. */
  public Rotation2d getLastTiltDirection() {
    return tiltDirection;
  }
}