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