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.trajectory.constraint;
006
007import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.math.controller.SimpleMotorFeedforward;
010import edu.wpi.first.math.geometry.Pose2d;
011import edu.wpi.first.math.kinematics.ChassisSpeeds;
012import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
013
014/**
015 * A class that enforces constraints on differential drive voltage expenditure based on the motor
016 * dynamics and the drive kinematics. Ensures that the acceleration of any wheel of the robot while
017 * following the trajectory is never higher than what can be achieved with the given maximum
018 * voltage.
019 */
020public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
021  private final SimpleMotorFeedforward m_feedforward;
022  private final DifferentialDriveKinematics m_kinematics;
023  private final double m_maxVoltage;
024
025  /**
026   * Creates a new DifferentialDriveVoltageConstraint.
027   *
028   * @param feedforward A feedforward component describing the behavior of the drive.
029   * @param kinematics A kinematics component describing the drive geometry.
030   * @param maxVoltage The maximum voltage available to the motors while following the path. Should
031   *     be somewhat less than the nominal battery voltage (12V) to account for "voltage sag" due to
032   *     current draw.
033   */
034  public DifferentialDriveVoltageConstraint(
035      SimpleMotorFeedforward feedforward,
036      DifferentialDriveKinematics kinematics,
037      double maxVoltage) {
038    m_feedforward =
039        requireNonNullParam(feedforward, "feedforward", "DifferentialDriveVoltageConstraint");
040    m_kinematics =
041        requireNonNullParam(kinematics, "kinematics", "DifferentialDriveVoltageConstraint");
042    m_maxVoltage = maxVoltage;
043  }
044
045  @Override
046  public double getMaxVelocityMetersPerSecond(
047      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
048    return Double.POSITIVE_INFINITY;
049  }
050
051  @Override
052  public MinMax getMinMaxAccelerationMetersPerSecondSq(
053      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
054    var wheelSpeeds =
055        m_kinematics.toWheelSpeeds(
056            new ChassisSpeeds(
057                velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter));
058
059    double maxWheelSpeed =
060        Math.max(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
061    double minWheelSpeed =
062        Math.min(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
063
064    // Calculate maximum/minimum possible accelerations from motor dynamics
065    // and max/min wheel speeds
066    double maxWheelAcceleration =
067        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
068    double minWheelAcceleration =
069        m_feedforward.minAchievableAcceleration(m_maxVoltage, minWheelSpeed);
070
071    // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
072    // increased by half of the trackwidth T.  Inner wheel has radius decreased
073    // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
074    // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2).
075    // Inner wheel is similar.
076
077    // sgn(speed) term added to correctly account for which wheel is on
078    // outside of turn:
079    // If moving forward, max acceleration constraint corresponds to wheel on outside of turn
080    // If moving backward, max acceleration constraint corresponds to wheel on inside of turn
081
082    // When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
083    // turning on its center) - we have to treat this as a special case, as it breaks
084    // the signum function.  Both max and min acceleration are *reduced in magnitude*
085    // in this case.
086
087    double maxChassisAcceleration;
088    double minChassisAcceleration;
089
090    if (velocityMetersPerSecond == 0) {
091      maxChassisAcceleration =
092          maxWheelAcceleration
093              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
094      minChassisAcceleration =
095          minWheelAcceleration
096              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
097    } else {
098      maxChassisAcceleration =
099          maxWheelAcceleration
100              / (1
101                  + m_kinematics.trackWidthMeters
102                      * Math.abs(curvatureRadPerMeter)
103                      * Math.signum(velocityMetersPerSecond)
104                      / 2);
105      minChassisAcceleration =
106          minWheelAcceleration
107              / (1
108                  - m_kinematics.trackWidthMeters
109                      * Math.abs(curvatureRadPerMeter)
110                      * Math.signum(velocityMetersPerSecond)
111                      / 2);
112    }
113
114    // When turning about a point inside the wheelbase (i.e. radius less than half
115    // the trackwidth), the inner wheel's direction changes, but the magnitude remains
116    // the same.  The formula above changes sign for the inner wheel when this happens.
117    // We can accurately account for this by simply negating the inner wheel.
118
119    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
120      if (velocityMetersPerSecond > 0) {
121        minChassisAcceleration = -minChassisAcceleration;
122      } else if (velocityMetersPerSecond < 0) {
123        maxChassisAcceleration = -maxChassisAcceleration;
124      }
125    }
126
127    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
128  }
129}