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. */
018  public double leftMetersPerSecond;
019
020  /** Speed of the right side of the robot. */
021  public double rightMetersPerSecond;
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 leftMetersPerSecond The left speed.
038   * @param rightMetersPerSecond The right speed.
039   */
040  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
041    this.leftMetersPerSecond = leftMetersPerSecond;
042    this.rightMetersPerSecond = rightMetersPerSecond;
043  }
044
045  /**
046   * Constructs a DifferentialDriveWheelSpeeds.
047   *
048   * @param left The left speed.
049   * @param right The right speed.
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 attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
064   */
065  public void desaturate(double attainableMaxSpeedMetersPerSecond) {
066    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
067
068    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
069      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
070      rightMetersPerSecond =
071          rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
072    }
073  }
074
075  /**
076   * Renormalizes the wheel speeds if any either side is above the specified maximum.
077   *
078   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
079   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
080   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
081   * absolute threshold, while maintaining the ratio of speeds between wheels.
082   *
083   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
084   */
085  public void desaturate(LinearVelocity attainableMaxSpeed) {
086    desaturate(attainableMaxSpeed.in(MetersPerSecond));
087  }
088
089  /**
090   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
091   *
092   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
093   * = DifferentialDriveWheelSpeeds{3.0, 2.0}
094   *
095   * @param other The DifferentialDriveWheelSpeeds to add.
096   * @return The sum of the DifferentialDriveWheelSpeeds.
097   */
098  public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
099    return new DifferentialDriveWheelSpeeds(
100        leftMetersPerSecond + other.leftMetersPerSecond,
101        rightMetersPerSecond + other.rightMetersPerSecond);
102  }
103
104  /**
105   * Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds
106   * and returns the difference.
107   *
108   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0}
109   * = DifferentialDriveWheelSpeeds{4.0, 2.0}
110   *
111   * @param other The DifferentialDriveWheelSpeeds to subtract.
112   * @return The difference between the two DifferentialDriveWheelSpeeds.
113   */
114  public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
115    return new DifferentialDriveWheelSpeeds(
116        leftMetersPerSecond - other.leftMetersPerSecond,
117        rightMetersPerSecond - other.rightMetersPerSecond);
118  }
119
120  /**
121   * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
122   * all components of the DifferentialDriveWheelSpeeds.
123   *
124   * @return The inverse of the current DifferentialDriveWheelSpeeds.
125   */
126  public DifferentialDriveWheelSpeeds unaryMinus() {
127    return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond);
128  }
129
130  /**
131   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
132   * DifferentialDriveWheelSpeeds.
133   *
134   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0,
135   * 5.0}
136   *
137   * @param scalar The scalar to multiply by.
138   * @return The scaled DifferentialDriveWheelSpeeds.
139   */
140  public DifferentialDriveWheelSpeeds times(double scalar) {
141    return new DifferentialDriveWheelSpeeds(
142        leftMetersPerSecond * scalar, rightMetersPerSecond * scalar);
143  }
144
145  /**
146   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
147   * DifferentialDriveWheelSpeeds.
148   *
149   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0,
150   * 1.25}
151   *
152   * @param scalar The scalar to divide by.
153   * @return The scaled DifferentialDriveWheelSpeeds.
154   */
155  public DifferentialDriveWheelSpeeds div(double scalar) {
156    return new DifferentialDriveWheelSpeeds(
157        leftMetersPerSecond / scalar, rightMetersPerSecond / scalar);
158  }
159
160  @Override
161  public String toString() {
162    return String.format(
163        "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
164        leftMetersPerSecond, rightMetersPerSecond);
165  }
166}