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.wpilibj.drive;
006
007import edu.wpi.first.math.MathUtil;
008import edu.wpi.first.wpilibj.MotorSafety;
009
010/** Common base class for drive platforms. */
011public abstract class RobotDriveBase extends MotorSafety {
012  public static final double kDefaultDeadband = 0.02;
013  public static final double kDefaultMaxOutput = 1.0;
014
015  protected double m_deadband = kDefaultDeadband;
016  protected double m_maxOutput = kDefaultMaxOutput;
017
018  /** The location of a motor on the robot for the purpose of driving. */
019  public enum MotorType {
020    kFrontLeft(0),
021    kFrontRight(1),
022    kRearLeft(2),
023    kRearRight(3),
024    kLeft(0),
025    kRight(1),
026    kBack(2);
027
028    public final int value;
029
030    MotorType(int value) {
031      this.value = value;
032    }
033  }
034
035  /** RobotDriveBase constructor. */
036  public RobotDriveBase() {
037    setSafetyEnabled(true);
038  }
039
040  /**
041   * Sets the deadband applied to the drive inputs (e.g., joystick values).
042   *
043   * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
044   * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link
045   * edu.wpi.first.math.MathUtil#applyDeadband}.
046   *
047   * @param deadband The deadband to set.
048   */
049  public void setDeadband(double deadband) {
050    m_deadband = deadband;
051  }
052
053  /**
054   * Configure the scaling factor for using drive methods with motor controllers in a mode other
055   * than PercentVbus or to limit the maximum output.
056   *
057   * <p>The default value is {@value #kDefaultMaxOutput}.
058   *
059   * @param maxOutput Multiplied with the output percentage computed by the drive functions.
060   */
061  public void setMaxOutput(double maxOutput) {
062    m_maxOutput = maxOutput;
063  }
064
065  /**
066   * Feed the motor safety object. Resets the timer that will stop the motors if it completes.
067   *
068   * @see MotorSafety#feed()
069   */
070  public void feedWatchdog() {
071    feed();
072  }
073
074  @Override
075  public abstract void stopMotor();
076
077  @Override
078  public abstract String getDescription();
079
080  /**
081   * Returns 0.0 if the given value is within the specified range around zero. The remaining range
082   * between the deadband and 1.0 is scaled from 0.0 to 1.0.
083   *
084   * @param value value to clip
085   * @param deadband range around zero
086   * @return The value after the deadband is applied.
087   * @deprecated Use MathUtil.applyDeadband(double,double).
088   */
089  @Deprecated(since = "2021", forRemoval = true)
090  protected static double applyDeadband(double value, double deadband) {
091    return MathUtil.applyDeadband(value, deadband);
092  }
093
094  /**
095   * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
096   *
097   * @param wheelSpeeds List of wheel speeds to normalize.
098   */
099  protected static void normalize(double[] wheelSpeeds) {
100    double maxMagnitude = Math.abs(wheelSpeeds[0]);
101    for (int i = 1; i < wheelSpeeds.length; i++) {
102      double temp = Math.abs(wheelSpeeds[i]);
103      if (maxMagnitude < temp) {
104        maxMagnitude = temp;
105      }
106    }
107    if (maxMagnitude > 1.0) {
108      for (int i = 0; i < wheelSpeeds.length; i++) {
109        wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
110      }
111    }
112  }
113}