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
007import static edu.wpi.first.units.Units.MetersPerSecond;
008
009import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelSpeedsProto;
010import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct;
011import edu.wpi.first.units.measure.LinearVelocity;
012import edu.wpi.first.util.protobuf.ProtobufSerializable;
013import edu.wpi.first.util.struct.StructSerializable;
014
015/** Represents the wheel speeds for a differential drive drivetrain. */
016public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
017  /** Speed of the left side of the robot in meters per second. */
018  public double left;
019
020  /** Speed of the right side of the robot in meters per second. */
021  public double right;
022
023  /** DifferentialDriveWheelSpeeds protobuf for serialization. */
024  public static final DifferentialDriveWheelSpeedsProto proto =
025      new DifferentialDriveWheelSpeedsProto();
026
027  /** DifferentialDriveWheelSpeeds struct for serialization. */
028  public static final DifferentialDriveWheelSpeedsStruct struct =
029      new DifferentialDriveWheelSpeedsStruct();
030
031  /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
032  public DifferentialDriveWheelSpeeds() {}
033
034  /**
035   * Constructs a DifferentialDriveWheelSpeeds.
036   *
037   * @param left The left speed in meters per second.
038   * @param right The right speed in meters per second.
039   */
040  public DifferentialDriveWheelSpeeds(double left, double right) {
041    this.left = left;
042    this.right = right;
043  }
044
045  /**
046   * Constructs a DifferentialDriveWheelSpeeds.
047   *
048   * @param left The left speed in meters per second.
049   * @param right The right speed in meters per second.
050   */
051  public DifferentialDriveWheelSpeeds(LinearVelocity left, LinearVelocity right) {
052    this(left.in(MetersPerSecond), right.in(MetersPerSecond));
053  }
054
055  /**
056   * Renormalizes the wheel speeds if any either side is above the specified maximum.
057   *
058   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
059   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
060   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
061   * absolute threshold, while maintaining the ratio of speeds between wheels.
062   *
063   * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
064   */
065  public void desaturate(double attainableMaxSpeed) {
066    double realMaxSpeed = Math.max(Math.abs(left), Math.abs(right));
067
068    if (realMaxSpeed > attainableMaxSpeed) {
069      left = left / realMaxSpeed * attainableMaxSpeed;
070      right = right / realMaxSpeed * attainableMaxSpeed;
071    }
072  }
073
074  /**
075   * Renormalizes the wheel speeds if any either side is above the specified maximum.
076   *
077   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
078   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
079   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
080   * absolute threshold, while maintaining the ratio of speeds between wheels.
081   *
082   * @param attainableMaxSpeed The absolute max speed in meters per second that a wheel can reach.
083   */
084  public void desaturate(LinearVelocity attainableMaxSpeed) {
085    desaturate(attainableMaxSpeed.in(MetersPerSecond));
086  }
087
088  /**
089   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
090   *
091   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
092   * = DifferentialDriveWheelSpeeds{3.0, 2.0}
093   *
094   * @param other The DifferentialDriveWheelSpeeds to add.
095   * @return The sum of the DifferentialDriveWheelSpeeds.
096   */
097  public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
098    return new DifferentialDriveWheelSpeeds(left + other.left, right + other.right);
099  }
100
101  /**
102   * Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds
103   * and returns the difference.
104   *
105   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0}
106   * = DifferentialDriveWheelSpeeds{4.0, 2.0}
107   *
108   * @param other The DifferentialDriveWheelSpeeds to subtract.
109   * @return The difference between the two DifferentialDriveWheelSpeeds.
110   */
111  public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
112    return new DifferentialDriveWheelSpeeds(left - other.left, right - other.right);
113  }
114
115  /**
116   * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
117   * all components of the DifferentialDriveWheelSpeeds.
118   *
119   * @return The inverse of the current DifferentialDriveWheelSpeeds.
120   */
121  public DifferentialDriveWheelSpeeds unaryMinus() {
122    return new DifferentialDriveWheelSpeeds(-left, -right);
123  }
124
125  /**
126   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
127   * DifferentialDriveWheelSpeeds.
128   *
129   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0,
130   * 5.0}
131   *
132   * @param scalar The scalar to multiply by.
133   * @return The scaled DifferentialDriveWheelSpeeds.
134   */
135  public DifferentialDriveWheelSpeeds times(double scalar) {
136    return new DifferentialDriveWheelSpeeds(left * scalar, right * scalar);
137  }
138
139  /**
140   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
141   * DifferentialDriveWheelSpeeds.
142   *
143   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0,
144   * 1.25}
145   *
146   * @param scalar The scalar to divide by.
147   * @return The scaled DifferentialDriveWheelSpeeds.
148   */
149  public DifferentialDriveWheelSpeeds div(double scalar) {
150    return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar);
151  }
152
153  @Override
154  public String toString() {
155    return String.format(
156        "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)", left, right);
157  }
158}