001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.kinematics;
006
007/** Represents the wheel speeds for a differential drive drivetrain. */
008@SuppressWarnings("MemberName")
009public class DifferentialDriveWheelSpeeds {
010  /** Speed of the left side of the robot. */
011  public double leftMetersPerSecond;
012
013  /** Speed of the right side of the robot. */
014  public double rightMetersPerSecond;
015
016  /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
017  public DifferentialDriveWheelSpeeds() {}
018
019  /**
020   * Constructs a DifferentialDriveWheelSpeeds.
021   *
022   * @param leftMetersPerSecond The left speed.
023   * @param rightMetersPerSecond The right speed.
024   */
025  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
026    this.leftMetersPerSecond = leftMetersPerSecond;
027    this.rightMetersPerSecond = rightMetersPerSecond;
028  }
029
030  /**
031   * Renormalizes the wheel speeds if any either side is above the specified maximum.
032   *
033   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
034   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
035   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
036   * absolute threshold, while maintaining the ratio of speeds between wheels.
037   *
038   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
039   */
040  public void desaturate(double attainableMaxSpeedMetersPerSecond) {
041    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
042
043    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
044      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
045      rightMetersPerSecond =
046          rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
047    }
048  }
049
050  @Override
051  public String toString() {
052    return String.format(
053        "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
054        leftMetersPerSecond, rightMetersPerSecond);
055  }
056}