001/*
002 * Copyright (C) Cross The Road Electronics.  All rights reserved.
003 * License information can be found in CTRE_LICENSE.txt
004 * For support and suggestions contact support@ctr-electronics.com or file
005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases
006 */
007package com.ctre.phoenix6.mechanisms.swerve.utility;
008
009import edu.wpi.first.math.MathSharedStore;
010import edu.wpi.first.math.MathUsageId;
011import edu.wpi.first.math.MathUtil;
012import edu.wpi.first.util.sendable.Sendable;
013import edu.wpi.first.util.sendable.SendableBuilder;
014import edu.wpi.first.util.sendable.SendableRegistry;
015
016/**
017 * Phoenix-centric PID controller taken from WPI's {@link edu.wpi.first.math.controller.PIDController} class.
018 * <p>
019 * This class differs from the WPI implementation by using explicit timestamps for
020 * integral/derivative calculations. Ideally, these timestamps come from the {@link com.ctre.phoenix6.StatusSignal}.
021 */
022public class PhoenixPIDController implements Sendable, AutoCloseable {
023  private static int instances;
024
025  // Factor for "proportional" control
026  private double m_kp;
027
028  // Factor for "integral" control
029  private double m_ki;
030
031  // Factor for "derivative" control
032  private double m_kd;
033
034  private double m_maximumIntegral = 1.0;
035
036  private double m_minimumIntegral = -1.0;
037
038  private double m_maximumInput;
039
040  private double m_minimumInput;
041
042  // Do the endpoints wrap around? e.g. Absolute encoder
043  private boolean m_continuous;
044
045  // The error at the time of the most recent call to calculate()
046  private double m_positionError;
047  private double m_velocityError;
048
049  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
050  private double m_prevError;
051
052  // The sum of the errors for use in the integral calc
053  private double m_totalError;
054
055  // The error that is considered at setpoint.
056  private double m_positionTolerance = 0.05;
057  private double m_velocityTolerance = Double.POSITIVE_INFINITY;
058
059  private double m_setpoint;
060  private double m_measurement;
061
062  private boolean m_haveMeasurement;
063  private boolean m_haveSetpoint;
064
065  private double m_lastAppliedOutput;
066
067  // The last timestamp acquired when performing a calculation
068  private double m_lastTimestamp;
069
070  /**
071   * Allocates a PIDController with the given constants for kp, ki, and kd.
072   *
073   * @param kp The proportional coefficient.
074   * @param ki The integral coefficient.
075   * @param kd The derivative coefficient.
076   */
077  public PhoenixPIDController(double kp, double ki, double kd) {
078    m_kp = kp;
079    m_ki = ki;
080    m_kd = kd;
081
082    instances++;
083    SendableRegistry.addLW(this, "PIDController", instances);
084
085    MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
086  }
087
088  @Override
089  public void close() {
090    SendableRegistry.remove(this);
091  }
092
093  /**
094   * Sets the PID Controller gain parameters.
095   *
096   * <p>Set the proportional, integral, and differential coefficients.
097   *
098   * @param kp The proportional coefficient.
099   * @param ki The integral coefficient.
100   * @param kd The derivative coefficient.
101   */
102  public void setPID(double kp, double ki, double kd) {
103    m_kp = kp;
104    m_ki = ki;
105    m_kd = kd;
106  }
107
108  /**
109   * Sets the Proportional coefficient of the PID controller gain.
110   *
111   * @param kp proportional coefficient
112   */
113  public void setP(double kp) {
114    m_kp = kp;
115  }
116
117  /**
118   * Sets the Integral coefficient of the PID controller gain.
119   *
120   * @param ki integral coefficient
121   */
122  public void setI(double ki) {
123    m_ki = ki;
124  }
125
126  /**
127   * Sets the Differential coefficient of the PID controller gain.
128   *
129   * @param kd differential coefficient
130   */
131  public void setD(double kd) {
132    m_kd = kd;
133  }
134
135  /**
136   * Get the Proportional coefficient.
137   *
138   * @return proportional coefficient
139   */
140  public double getP() {
141    return m_kp;
142  }
143
144  /**
145   * Get the Integral coefficient.
146   *
147   * @return integral coefficient
148   */
149  public double getI() {
150    return m_ki;
151  }
152
153  /**
154   * Get the Differential coefficient.
155   *
156   * @return differential coefficient
157   */
158  public double getD() {
159    return m_kd;
160  }
161
162  /**
163   * Returns the position tolerance of this controller.
164   *
165   * @return the position tolerance of the controller.
166   */
167  public double getPositionTolerance() {
168    return m_positionTolerance;
169  }
170
171  /**
172   * Returns the velocity tolerance of this controller.
173   *
174   * @return the velocity tolerance of the controller.
175   */
176  public double getVelocityTolerance() {
177    return m_velocityTolerance;
178  }
179
180  /**
181   * Returns the current setpoint of the PIDController.
182   *
183   * @return The current setpoint.
184   */
185  public double getSetpoint() {
186    return m_setpoint;
187  }
188
189  /**
190   * Returns true if the error is within the tolerance of the setpoint.
191   *
192   * <p>This will return false until at least one input value has been computed.
193   *
194   * @return Whether the error is within the acceptable bounds.
195   */
196  public boolean atSetpoint() {
197    return m_haveMeasurement
198        && m_haveSetpoint
199        && Math.abs(m_positionError) < m_positionTolerance
200        && Math.abs(m_velocityError) < m_velocityTolerance;
201  }
202
203  /**
204   * Enables continuous input.
205   *
206   * <p>Rather then using the max and min input range as constraints, it considers them to be the
207   * same point and automatically calculates the shortest route to the setpoint.
208   *
209   * @param minimumInput The minimum value expected from the input.
210   * @param maximumInput The maximum value expected from the input.
211   */
212  public void enableContinuousInput(double minimumInput, double maximumInput) {
213    m_continuous = true;
214    m_minimumInput = minimumInput;
215    m_maximumInput = maximumInput;
216  }
217
218  /** Disables continuous input. */
219  public void disableContinuousInput() {
220    m_continuous = false;
221  }
222
223  /**
224   * Returns true if continuous input is enabled.
225   *
226   * @return True if continuous input is enabled.
227   */
228  public boolean isContinuousInputEnabled() {
229    return m_continuous;
230  }
231
232  /**
233   * Sets the minimum and maximum values for the integrator.
234   *
235   * <p>When the cap is reached, the integrator value is added to the controller output rather than
236   * the integrator value times the integral gain.
237   *
238   * @param minimumIntegral The minimum value of the integrator.
239   * @param maximumIntegral The maximum value of the integrator.
240   */
241  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
242    m_minimumIntegral = minimumIntegral;
243    m_maximumIntegral = maximumIntegral;
244  }
245
246  /**
247   * Sets the error which is considered tolerable for use with atSetpoint().
248   *
249   * @param positionTolerance Position error which is tolerable.
250   */
251  public void setTolerance(double positionTolerance) {
252    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
253  }
254
255  /**
256   * Sets the error which is considered tolerable for use with atSetpoint().
257   *
258   * @param positionTolerance Position error which is tolerable.
259   * @param velocityTolerance Velocity error which is tolerable.
260   */
261  public void setTolerance(double positionTolerance, double velocityTolerance) {
262    m_positionTolerance = positionTolerance;
263    m_velocityTolerance = velocityTolerance;
264  }
265
266  /**
267   * Returns the difference between the setpoint and the measurement.
268   *
269   * @return The error.
270   */
271  public double getPositionError() {
272    return m_positionError;
273  }
274
275  /**
276   * Returns the velocity error.
277   *
278   * @return The velocity error.
279   */
280  public double getVelocityError() {
281    return m_velocityError;
282  }
283
284  /**
285   * Returns the next output of the PID controller.
286   *
287   * @param measurement The current measurement of the process variable.
288   * @param setpoint The setpoint to target
289   * @param currentTimestamp The current timestamp to use for calculating integral/derivative error
290   * @return The next controller output.
291   */
292  public double calculate(double measurement, double setpoint, double currentTimestamp) {
293    m_setpoint = setpoint;
294    m_haveSetpoint = true;
295    m_measurement = measurement;
296    m_prevError = m_positionError;
297    m_haveMeasurement = true;
298
299    double thisPeriod = currentTimestamp - m_lastTimestamp;
300    m_lastTimestamp = currentTimestamp;
301
302    if (m_continuous) {
303      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
304      m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
305    } else {
306      m_positionError = m_setpoint - m_measurement;
307    }
308
309    m_velocityError = (m_positionError - m_prevError) / thisPeriod;
310
311    if (m_ki != 0) {
312      m_totalError =
313          MathUtil.clamp(
314              m_totalError + m_positionError * thisPeriod,
315              m_minimumIntegral / m_ki,
316              m_maximumIntegral / m_ki);
317    }
318
319    m_lastAppliedOutput = m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError;
320    return m_lastAppliedOutput;
321  }
322
323  /* Get the last applied output from this PID controller */
324  public double getLastAppliedOutput() {
325    return m_lastAppliedOutput;
326  }
327
328  /** Resets the previous error and the integral term. */
329  public void reset() {
330    m_positionError = 0;
331    m_prevError = 0;
332    m_totalError = 0;
333    m_velocityError = 0;
334    m_haveMeasurement = false;
335  }
336
337  @Override
338  public void initSendable(SendableBuilder builder) {
339    builder.setSmartDashboardType("PIDController");
340    builder.addDoubleProperty("p", this::getP, this::setP);
341    builder.addDoubleProperty("i", this::getI, this::setI);
342    builder.addDoubleProperty("d", this::getD, this::setD);
343    builder.addDoubleProperty("setpoint", this::getSetpoint, null);
344  }
345}