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.motorcontrol;
006
007import edu.wpi.first.util.sendable.Sendable;
008import edu.wpi.first.util.sendable.SendableBuilder;
009import edu.wpi.first.util.sendable.SendableRegistry;
010import edu.wpi.first.wpilibj.MotorSafety;
011import edu.wpi.first.wpilibj.PWM;
012import edu.wpi.first.wpilibj.RobotController;
013import java.util.ArrayList;
014
015/** Common base class for all PWM Motor Controllers. */
016@SuppressWarnings("removal")
017public abstract class PWMMotorController extends MotorSafety
018    implements MotorController, Sendable, AutoCloseable {
019  private boolean m_isInverted;
020  private final ArrayList<PWMMotorController> m_followers = new ArrayList<>();
021
022  /** PWM instances for motor controller. */
023  protected PWM m_pwm;
024
025  /**
026   * Constructor.
027   *
028   * @param name Name to use for SendableRegistry
029   * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
030   *     on the MXP port
031   */
032  @SuppressWarnings("this-escape")
033  protected PWMMotorController(final String name, final int channel) {
034    m_pwm = new PWM(channel, false);
035    SendableRegistry.addLW(this, name, channel);
036  }
037
038  /** Free the resource associated with the PWM channel and set the value to 0. */
039  @Override
040  public void close() {
041    SendableRegistry.remove(this);
042    m_pwm.close();
043  }
044
045  /**
046   * Set the PWM value.
047   *
048   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
049   * FPGA.
050   *
051   * @param speed The speed value between -1.0 and 1.0 to set.
052   */
053  @Override
054  public void set(double speed) {
055    if (m_isInverted) {
056      speed = -speed;
057    }
058    m_pwm.setSpeed(speed);
059
060    for (var follower : m_followers) {
061      follower.set(speed);
062    }
063
064    feed();
065  }
066
067  /**
068   * Get the recently set value of the PWM. This value is affected by the inversion property. If you
069   * want the value that is sent directly to the MotorController, use {@link
070   * edu.wpi.first.wpilibj.PWM#getSpeed()} instead.
071   *
072   * @return The most recently set value for the PWM between -1.0 and 1.0.
073   */
074  @Override
075  public double get() {
076    return m_pwm.getSpeed() * (m_isInverted ? -1.0 : 1.0);
077  }
078
079  /**
080   * Gets the voltage output of the motor controller, nominally between -12 V and 12 V.
081   *
082   * @return The voltage of the motor controller, nominally between -12 V and 12 V.
083   */
084  public double getVoltage() {
085    return get() * RobotController.getBatteryVoltage();
086  }
087
088  @Override
089  public void setInverted(boolean isInverted) {
090    m_isInverted = isInverted;
091  }
092
093  @Override
094  public boolean getInverted() {
095    return m_isInverted;
096  }
097
098  @Override
099  public void disable() {
100    m_pwm.setDisabled();
101
102    for (var follower : m_followers) {
103      follower.disable();
104    }
105  }
106
107  @Override
108  public void stopMotor() {
109    // Don't use set(0) as that will feed the watch kitty
110    m_pwm.setSpeed(0);
111
112    for (var follower : m_followers) {
113      follower.stopMotor();
114    }
115  }
116
117  @Override
118  public String getDescription() {
119    return "PWM " + getChannel();
120  }
121
122  /**
123   * Gets the backing PWM handle.
124   *
125   * @return The pwm handle.
126   */
127  public int getPwmHandle() {
128    return m_pwm.getHandle();
129  }
130
131  /**
132   * Gets the PWM channel number.
133   *
134   * @return The channel number.
135   */
136  public int getChannel() {
137    return m_pwm.getChannel();
138  }
139
140  /**
141   * Optionally eliminate the deadband from a motor controller.
142   *
143   * @param eliminateDeadband If true, set the motor curve for the motor controller to eliminate the
144   *     deadband in the middle of the range. Otherwise, keep the full range without modifying any
145   *     values.
146   */
147  public void enableDeadbandElimination(boolean eliminateDeadband) {
148    m_pwm.enableDeadbandElimination(eliminateDeadband);
149  }
150
151  /**
152   * Make the given PWM motor controller follow the output of this one.
153   *
154   * @param follower The motor controller follower.
155   */
156  public void addFollower(PWMMotorController follower) {
157    m_followers.add(follower);
158  }
159
160  @Override
161  public void initSendable(SendableBuilder builder) {
162    builder.setSmartDashboardType("Motor Controller");
163    builder.setActuator(true);
164    builder.setSafeState(this::disable);
165    builder.addDoubleProperty("Value", this::get, this::set);
166  }
167}