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. */
008public class DifferentialDriveWheelSpeeds {
009  /** Speed of the left side of the robot. */
010  public double leftMetersPerSecond;
011
012  /** Speed of the right side of the robot. */
013  public double rightMetersPerSecond;
014
015  /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
016  public DifferentialDriveWheelSpeeds() {}
017
018  /**
019   * Constructs a DifferentialDriveWheelSpeeds.
020   *
021   * @param leftMetersPerSecond The left speed.
022   * @param rightMetersPerSecond The right speed.
023   */
024  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
025    this.leftMetersPerSecond = leftMetersPerSecond;
026    this.rightMetersPerSecond = rightMetersPerSecond;
027  }
028
029  /**
030   * Renormalizes the wheel speeds if any either side is above the specified maximum.
031   *
032   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
033   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
034   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
035   * absolute threshold, while maintaining the ratio of speeds between wheels.
036   *
037   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
038   */
039  public void desaturate(double attainableMaxSpeedMetersPerSecond) {
040    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
041
042    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
043      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
044      rightMetersPerSecond =
045          rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
046    }
047  }
048
049  @Override
050  public String toString() {
051    return String.format(
052        "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
053        leftMetersPerSecond, rightMetersPerSecond);
054  }
055}