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  public RobotDriveBase() {
040    setSafetyEnabled(true);
041  }
042
043  /**
044   * Sets the deadband applied to the drive inputs (e.g., joystick values).
045   *
046   * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
047   * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link
048   * edu.wpi.first.math.MathUtil#applyDeadband}.
049   *
050   * @param deadband The deadband to set.
051   */
052  public void setDeadband(double deadband) {
053    m_deadband = deadband;
054  }
055
056  /**
057   * Configure the scaling factor for using drive methods with motor controllers in a mode other
058   * than PercentVbus or to limit the maximum output.
059   *
060   * <p>The default value is {@value #kDefaultMaxOutput}.
061   *
062   * @param maxOutput Multiplied with the output percentage computed by the drive functions.
063   */
064  public void setMaxOutput(double maxOutput) {
065    m_maxOutput = maxOutput;
066  }
067
068  /**
069   * Feed the motor safety object. Resets the timer that will stop the motors if it completes.
070   *
071   * @see MotorSafety#feed()
072   */
073  public void feedWatchdog() {
074    feed();
075  }
076
077  @Override
078  public abstract void stopMotor();
079
080  @Override
081  public abstract String getDescription();
082
083  /**
084   * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
085   *
086   * @param wheelSpeeds List of wheel speeds to normalize.
087   */
088  protected static void normalize(double[] wheelSpeeds) {
089    double maxMagnitude = Math.abs(wheelSpeeds[0]);
090    for (int i = 1; i < wheelSpeeds.length; i++) {
091      double temp = Math.abs(wheelSpeeds[i]);
092      if (maxMagnitude < temp) {
093        maxMagnitude = temp;
094      }
095    }
096    if (maxMagnitude > 1.0) {
097      for (int i = 0; i < wheelSpeeds.length; i++) {
098        wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
099      }
100    }
101  }
102}