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.hal.SimDevice;
008import edu.wpi.first.hal.SimDevice.Direction;
009import edu.wpi.first.hal.SimDouble;
010import edu.wpi.first.math.MathUtil;
011import edu.wpi.first.util.sendable.Sendable;
012import edu.wpi.first.util.sendable.SendableBuilder;
013import edu.wpi.first.util.sendable.SendableRegistry;
014import edu.wpi.first.wpilibj.MotorSafety;
015import edu.wpi.first.wpilibj.PWM;
016import edu.wpi.first.wpilibj.RobotController;
017import java.util.ArrayList;
018
019/** Common base class for all PWM Motor Controllers. */
020@SuppressWarnings("removal")
021public abstract class PWMMotorController extends MotorSafety
022    implements MotorController, Sendable, AutoCloseable {
023  private boolean m_isInverted;
024  private final ArrayList<PWMMotorController> m_followers = new ArrayList<>();
025
026  /** PWM instances for motor controller. */
027  protected PWM m_pwm;
028
029  private SimDevice m_simDevice;
030  private SimDouble m_simSpeed;
031
032  private boolean m_eliminateDeadband;
033  private int m_minPwm;
034  private int m_deadbandMinPwm;
035  private int m_centerPwm;
036  private int m_deadbandMaxPwm;
037  private int m_maxPwm;
038
039  /**
040   * Constructor.
041   *
042   * @param name Name to use for SendableRegistry
043   * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
044   *     on the MXP port
045   */
046  @SuppressWarnings("this-escape")
047  protected PWMMotorController(final String name, final int channel) {
048    m_pwm = new PWM(channel, false);
049    SendableRegistry.add(this, name, channel);
050
051    m_simDevice = SimDevice.create("PWMMotorController", channel);
052    if (m_simDevice != null) {
053      m_simSpeed = m_simDevice.createDouble("Speed", Direction.kOutput, 0.0);
054      m_pwm.setSimDevice(m_simDevice);
055    }
056  }
057
058  /** Free the resource associated with the PWM channel and set the value to 0. */
059  @Override
060  public void close() {
061    SendableRegistry.remove(this);
062    m_pwm.close();
063
064    if (m_simDevice != null) {
065      m_simDevice.close();
066      m_simDevice = null;
067      m_simSpeed = null;
068    }
069  }
070
071  private int getMinPositivePwm() {
072    if (m_eliminateDeadband) {
073      return m_deadbandMaxPwm;
074    } else {
075      return m_centerPwm + 1;
076    }
077  }
078
079  private int getMaxNegativePwm() {
080    if (m_eliminateDeadband) {
081      return m_deadbandMinPwm;
082    } else {
083      return m_centerPwm - 1;
084    }
085  }
086
087  private int getPositiveScaleFactor() {
088    return m_maxPwm - getMinPositivePwm();
089  }
090
091  private int getNegativeScaleFactor() {
092    return getMaxNegativePwm() - m_minPwm;
093  }
094
095  /**
096   * Takes a speed from -1 to 1, and outputs it in the microsecond format.
097   *
098   * @param speed the speed to output
099   */
100  protected final void setSpeed(double speed) {
101    if (Double.isFinite(speed)) {
102      speed = MathUtil.clamp(speed, -1.0, 1.0);
103    } else {
104      speed = 0.0;
105    }
106
107    if (m_simSpeed != null) {
108      m_simSpeed.set(speed);
109    }
110
111    int rawValue;
112    if (speed == 0.0) {
113      rawValue = m_centerPwm;
114    } else if (speed > 0.0) {
115      rawValue = (int) Math.round(speed * getPositiveScaleFactor()) + getMinPositivePwm();
116    } else {
117      rawValue = (int) Math.round(speed * getNegativeScaleFactor()) + getMaxNegativePwm();
118    }
119
120    m_pwm.setPulseTimeMicroseconds(rawValue);
121  }
122
123  /**
124   * Gets the speed from -1 to 1, from the currently set pulse time.
125   *
126   * @return motor controller speed
127   */
128  protected final double getSpeed() {
129    int rawValue = m_pwm.getPulseTimeMicroseconds();
130
131    if (rawValue == 0) {
132      return 0.0;
133    } else if (rawValue > m_maxPwm) {
134      return 1.0;
135    } else if (rawValue < m_minPwm) {
136      return -1.0;
137    } else if (rawValue > getMinPositivePwm()) {
138      return (rawValue - getMinPositivePwm()) / (double) getPositiveScaleFactor();
139    } else if (rawValue < getMaxNegativePwm()) {
140      return (rawValue - getMaxNegativePwm()) / (double) getNegativeScaleFactor();
141    } else {
142      return 0.0;
143    }
144  }
145
146  /**
147   * Sets the bounds in microseconds for the controller.
148   *
149   * @param maxPwm maximum
150   * @param deadbandMaxPwm deadband max
151   * @param centerPwm center
152   * @param deadbandMinPwm deadmand min
153   * @param minPwm minimum
154   */
155  protected final void setBoundsMicroseconds(
156      int maxPwm, int deadbandMaxPwm, int centerPwm, int deadbandMinPwm, int minPwm) {
157    m_maxPwm = maxPwm;
158    m_deadbandMaxPwm = deadbandMaxPwm;
159    m_centerPwm = centerPwm;
160    m_deadbandMinPwm = deadbandMinPwm;
161    m_minPwm = minPwm;
162  }
163
164  /**
165   * Set the PWM value.
166   *
167   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
168   * FPGA.
169   *
170   * @param speed The speed value between -1.0 and 1.0 to set.
171   */
172  @Override
173  public void set(double speed) {
174    if (m_isInverted) {
175      speed = -speed;
176    }
177    setSpeed(speed);
178
179    for (var follower : m_followers) {
180      follower.set(speed);
181    }
182
183    feed();
184  }
185
186  /**
187   * Get the recently set value of the PWM. This value is affected by the inversion property.
188   *
189   * @return The most recently set value for the PWM between -1.0 and 1.0.
190   */
191  @Override
192  public double get() {
193    return getSpeed() * (m_isInverted ? -1.0 : 1.0);
194  }
195
196  /**
197   * Gets the voltage output of the motor controller, nominally between -12 V and 12 V.
198   *
199   * @return The voltage of the motor controller, nominally between -12 V and 12 V.
200   */
201  public double getVoltage() {
202    return get() * RobotController.getBatteryVoltage();
203  }
204
205  @Override
206  public void setInverted(boolean isInverted) {
207    m_isInverted = isInverted;
208  }
209
210  @Override
211  public boolean getInverted() {
212    return m_isInverted;
213  }
214
215  @Override
216  public void disable() {
217    m_pwm.setDisabled();
218
219    if (m_simSpeed != null) {
220      m_simSpeed.set(0.0);
221    }
222
223    for (var follower : m_followers) {
224      follower.disable();
225    }
226  }
227
228  @Override
229  public void stopMotor() {
230    // Don't use set(0) as that will feed the watch kitty
231    m_pwm.setPulseTimeMicroseconds(0);
232
233    for (var follower : m_followers) {
234      follower.stopMotor();
235    }
236  }
237
238  @Override
239  public String getDescription() {
240    return "PWM " + getChannel();
241  }
242
243  /**
244   * Gets the backing PWM handle.
245   *
246   * @return The pwm handle.
247   */
248  public int getPwmHandle() {
249    return m_pwm.getHandle();
250  }
251
252  /**
253   * Gets the PWM channel number.
254   *
255   * @return The channel number.
256   */
257  public int getChannel() {
258    return m_pwm.getChannel();
259  }
260
261  /**
262   * Optionally eliminate the deadband from a motor controller.
263   *
264   * @param eliminateDeadband If true, set the motor curve for the motor controller to eliminate the
265   *     deadband in the middle of the range. Otherwise, keep the full range without modifying any
266   *     values.
267   */
268  public void enableDeadbandElimination(boolean eliminateDeadband) {
269    m_eliminateDeadband = eliminateDeadband;
270  }
271
272  /**
273   * Make the given PWM motor controller follow the output of this one.
274   *
275   * @param follower The motor controller follower.
276   */
277  public void addFollower(PWMMotorController follower) {
278    m_followers.add(follower);
279  }
280
281  @Override
282  public void initSendable(SendableBuilder builder) {
283    builder.setSmartDashboardType("Motor Controller");
284    builder.setActuator(true);
285    builder.addDoubleProperty("Value", this::get, this::set);
286  }
287}