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