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