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.math.controller; 006 007import edu.wpi.first.math.MathSharedStore; 008import edu.wpi.first.math.MathUtil; 009import edu.wpi.first.util.sendable.Sendable; 010import edu.wpi.first.util.sendable.SendableBuilder; 011import edu.wpi.first.util.sendable.SendableRegistry; 012 013/** Implements a PID control loop. */ 014public class PIDController implements Sendable, AutoCloseable { 015 private static int instances; 016 017 // Factor for "proportional" control 018 private double m_kp; 019 020 // Factor for "integral" control 021 private double m_ki; 022 023 // Factor for "derivative" control 024 private double m_kd; 025 026 // The error range where "integral" control applies 027 private double m_iZone = Double.POSITIVE_INFINITY; 028 029 // The period (in seconds) of the loop that calls the controller 030 private final double m_period; 031 032 private double m_maximumIntegral = 1.0; 033 034 private double m_minimumIntegral = -1.0; 035 036 private double m_maximumInput; 037 038 private double m_minimumInput; 039 040 // Do the endpoints wrap around? e.g. Absolute encoder 041 private boolean m_continuous; 042 043 // The error at the time of the most recent call to calculate() 044 private double m_error; 045 private double m_errorDerivative; 046 047 // The error at the time of the second-most-recent call to calculate() (used to compute velocity) 048 private double m_prevError; 049 050 // The sum of the errors for use in the integral calc 051 private double m_totalError; 052 053 // The error that is considered at setpoint. 054 private double m_errorTolerance = 0.05; 055 private double m_errorDerivativeTolerance = Double.POSITIVE_INFINITY; 056 057 private double m_setpoint; 058 private double m_measurement; 059 060 private boolean m_haveMeasurement; 061 private boolean m_haveSetpoint; 062 063 /** 064 * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of 065 * 0.02 seconds. 066 * 067 * @param kp The proportional coefficient. 068 * @param ki The integral coefficient. 069 * @param kd The derivative coefficient. 070 * @throws IllegalArgumentException if kp < 0 071 * @throws IllegalArgumentException if ki < 0 072 * @throws IllegalArgumentException if kd < 0 073 */ 074 public PIDController(double kp, double ki, double kd) { 075 this(kp, ki, kd, 0.02); 076 } 077 078 /** 079 * Allocates a PIDController with the given constants for kp, ki, and kd. 080 * 081 * @param kp The proportional coefficient. 082 * @param ki The integral coefficient. 083 * @param kd The derivative coefficient. 084 * @param period The period between controller updates in seconds. 085 * @throws IllegalArgumentException if kp < 0 086 * @throws IllegalArgumentException if ki < 0 087 * @throws IllegalArgumentException if kd < 0 088 * @throws IllegalArgumentException if period <= 0 089 */ 090 @SuppressWarnings("this-escape") 091 public PIDController(double kp, double ki, double kd, double period) { 092 m_kp = kp; 093 m_ki = ki; 094 m_kd = kd; 095 096 if (kp < 0.0) { 097 throw new IllegalArgumentException("Kp must be a non-negative number!"); 098 } 099 if (ki < 0.0) { 100 throw new IllegalArgumentException("Ki must be a non-negative number!"); 101 } 102 if (kd < 0.0) { 103 throw new IllegalArgumentException("Kd must be a non-negative number!"); 104 } 105 if (period <= 0.0) { 106 throw new IllegalArgumentException("Controller period must be a positive number!"); 107 } 108 m_period = period; 109 110 instances++; 111 SendableRegistry.add(this, "PIDController", instances); 112 113 MathSharedStore.reportUsage("PIDController", String.valueOf(instances)); 114 } 115 116 @Override 117 public void close() { 118 SendableRegistry.remove(this); 119 } 120 121 /** 122 * Sets the PID Controller gain parameters. 123 * 124 * <p>Set the proportional, integral, and differential coefficients. 125 * 126 * @param kp The proportional coefficient. 127 * @param ki The integral coefficient. 128 * @param kd The derivative coefficient. 129 */ 130 public void setPID(double kp, double ki, double kd) { 131 m_kp = kp; 132 m_ki = ki; 133 m_kd = kd; 134 } 135 136 /** 137 * Sets the Proportional coefficient of the PID controller gain. 138 * 139 * @param kp The proportional coefficient. Must be >= 0. 140 */ 141 public void setP(double kp) { 142 m_kp = kp; 143 } 144 145 /** 146 * Sets the Integral coefficient of the PID controller gain. 147 * 148 * @param ki The integral coefficient. Must be >= 0. 149 */ 150 public void setI(double ki) { 151 m_ki = ki; 152 } 153 154 /** 155 * Sets the Differential coefficient of the PID controller gain. 156 * 157 * @param kd The differential coefficient. Must be >= 0. 158 */ 159 public void setD(double kd) { 160 m_kd = kd; 161 } 162 163 /** 164 * Sets the IZone range. When the absolute value of the position error is greater than IZone, the 165 * total accumulated error will reset to zero, disabling integral gain until the absolute value of 166 * the position error is less than IZone. This is used to prevent integral windup. Must be 167 * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value 168 * of {@link Double#POSITIVE_INFINITY} disables IZone functionality. 169 * 170 * @param iZone Maximum magnitude of error to allow integral control. 171 * @throws IllegalArgumentException if iZone < 0 172 */ 173 public void setIZone(double iZone) { 174 if (iZone < 0) { 175 throw new IllegalArgumentException("IZone must be a non-negative number!"); 176 } 177 m_iZone = iZone; 178 } 179 180 /** 181 * Get the Proportional coefficient. 182 * 183 * @return proportional coefficient 184 */ 185 public double getP() { 186 return m_kp; 187 } 188 189 /** 190 * Get the Integral coefficient. 191 * 192 * @return integral coefficient 193 */ 194 public double getI() { 195 return m_ki; 196 } 197 198 /** 199 * Get the Differential coefficient. 200 * 201 * @return differential coefficient 202 */ 203 public double getD() { 204 return m_kd; 205 } 206 207 /** 208 * Get the IZone range. 209 * 210 * @return Maximum magnitude of error to allow integral control. 211 */ 212 public double getIZone() { 213 return m_iZone; 214 } 215 216 /** 217 * Returns the period of this controller. 218 * 219 * @return the period of the controller. 220 */ 221 public double getPeriod() { 222 return m_period; 223 } 224 225 /** 226 * Returns the position tolerance of this controller. 227 * 228 * @return the position tolerance of the controller. 229 * @deprecated Use getErrorTolerance() instead. 230 */ 231 @Deprecated(forRemoval = true, since = "2025") 232 public double getPositionTolerance() { 233 return m_errorTolerance; 234 } 235 236 /** 237 * Returns the velocity tolerance of this controller. 238 * 239 * @return the velocity tolerance of the controller. 240 * @deprecated Use getErrorDerivativeTolerance() instead. 241 */ 242 @Deprecated(forRemoval = true, since = "2025") 243 public double getVelocityTolerance() { 244 return m_errorDerivativeTolerance; 245 } 246 247 /** 248 * Returns the error tolerance of this controller. Defaults to 0.05. 249 * 250 * @return the error tolerance of the controller. 251 */ 252 public double getErrorTolerance() { 253 return m_errorTolerance; 254 } 255 256 /** 257 * Returns the error derivative tolerance of this controller. Defaults to ∞. 258 * 259 * @return the error derivative tolerance of the controller. 260 */ 261 public double getErrorDerivativeTolerance() { 262 return m_errorDerivativeTolerance; 263 } 264 265 /** 266 * Returns the accumulated error used in the integral calculation of this controller. 267 * 268 * @return The accumulated error of this controller. 269 */ 270 public double getAccumulatedError() { 271 return m_totalError; 272 } 273 274 /** 275 * Sets the setpoint for the PIDController. 276 * 277 * @param setpoint The desired setpoint. 278 */ 279 public void setSetpoint(double setpoint) { 280 m_setpoint = setpoint; 281 m_haveSetpoint = true; 282 283 if (m_continuous) { 284 double errorBound = (m_maximumInput - m_minimumInput) / 2.0; 285 m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); 286 } else { 287 m_error = m_setpoint - m_measurement; 288 } 289 290 m_errorDerivative = (m_error - m_prevError) / m_period; 291 } 292 293 /** 294 * Returns the current setpoint of the PIDController. 295 * 296 * @return The current setpoint. 297 */ 298 public double getSetpoint() { 299 return m_setpoint; 300 } 301 302 /** 303 * Returns true if the error is within the tolerance of the setpoint. The error tolerance defaults 304 * to 0.05, and the error derivative tolerance defaults to ∞. 305 * 306 * <p>This will return false until at least one input value has been computed. 307 * 308 * @return Whether the error is within the acceptable bounds. 309 */ 310 public boolean atSetpoint() { 311 return m_haveMeasurement 312 && m_haveSetpoint 313 && Math.abs(m_error) < m_errorTolerance 314 && Math.abs(m_errorDerivative) < m_errorDerivativeTolerance; 315 } 316 317 /** 318 * Enables continuous input. 319 * 320 * <p>Rather then using the max and min input range as constraints, it considers them to be the 321 * same point and automatically calculates the shortest route to the setpoint. 322 * 323 * @param minimumInput The minimum value expected from the input. 324 * @param maximumInput The maximum value expected from the input. 325 */ 326 public void enableContinuousInput(double minimumInput, double maximumInput) { 327 m_continuous = true; 328 m_minimumInput = minimumInput; 329 m_maximumInput = maximumInput; 330 } 331 332 /** Disables continuous input. */ 333 public void disableContinuousInput() { 334 m_continuous = false; 335 } 336 337 /** 338 * Returns true if continuous input is enabled. 339 * 340 * @return True if continuous input is enabled. 341 */ 342 public boolean isContinuousInputEnabled() { 343 return m_continuous; 344 } 345 346 /** 347 * Sets the minimum and maximum contributions of the integral term. 348 * 349 * <p>The internal integrator is clamped so that the integral term's contribution to the output 350 * stays between minimumIntegral and maximumIntegral. This prevents integral windup. 351 * 352 * @param minimumIntegral The minimum contribution of the integral term. 353 * @param maximumIntegral The maximum contribution of the integral term. 354 */ 355 public void setIntegratorRange(double minimumIntegral, double maximumIntegral) { 356 m_minimumIntegral = minimumIntegral; 357 m_maximumIntegral = maximumIntegral; 358 } 359 360 /** 361 * Sets the error which is considered tolerable for use with atSetpoint(). 362 * 363 * @param errorTolerance Error which is tolerable. 364 */ 365 public void setTolerance(double errorTolerance) { 366 setTolerance(errorTolerance, Double.POSITIVE_INFINITY); 367 } 368 369 /** 370 * Sets the error which is considered tolerable for use with atSetpoint(). 371 * 372 * @param errorTolerance Error which is tolerable. 373 * @param errorDerivativeTolerance Error derivative which is tolerable. 374 */ 375 public void setTolerance(double errorTolerance, double errorDerivativeTolerance) { 376 m_errorTolerance = errorTolerance; 377 m_errorDerivativeTolerance = errorDerivativeTolerance; 378 } 379 380 /** 381 * Returns the difference between the setpoint and the measurement. 382 * 383 * @return The error. 384 * @deprecated Use getError() instead. 385 */ 386 @Deprecated(forRemoval = true, since = "2025") 387 public double getPositionError() { 388 return m_error; 389 } 390 391 /** 392 * Returns the velocity error. 393 * 394 * @return The velocity error. 395 * @deprecated Use getErrorDerivative() instead. 396 */ 397 @Deprecated(forRemoval = true, since = "2025") 398 public double getVelocityError() { 399 return m_errorDerivative; 400 } 401 402 /** 403 * Returns the difference between the setpoint and the measurement. 404 * 405 * @return The error. 406 */ 407 public double getError() { 408 return m_error; 409 } 410 411 /** 412 * Returns the error derivative. 413 * 414 * @return The error derivative. 415 */ 416 public double getErrorDerivative() { 417 return m_errorDerivative; 418 } 419 420 /** 421 * Returns the next output of the PID controller. 422 * 423 * @param measurement The current measurement of the process variable. 424 * @param setpoint The new setpoint of the controller. 425 * @return The next controller output. 426 */ 427 public double calculate(double measurement, double setpoint) { 428 m_setpoint = setpoint; 429 m_haveSetpoint = true; 430 return calculate(measurement); 431 } 432 433 /** 434 * Returns the next output of the PID controller. 435 * 436 * @param measurement The current measurement of the process variable. 437 * @return The next controller output. 438 */ 439 public double calculate(double measurement) { 440 m_measurement = measurement; 441 m_prevError = m_error; 442 m_haveMeasurement = true; 443 444 if (m_continuous) { 445 double errorBound = (m_maximumInput - m_minimumInput) / 2.0; 446 m_error = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); 447 } else { 448 m_error = m_setpoint - m_measurement; 449 } 450 451 m_errorDerivative = (m_error - m_prevError) / m_period; 452 453 // If the absolute value of the position error is greater than IZone, reset the total error 454 if (Math.abs(m_error) > m_iZone) { 455 m_totalError = 0; 456 } else if (m_ki != 0) { 457 m_totalError = 458 MathUtil.clamp( 459 m_totalError + m_error * m_period, 460 m_minimumIntegral / m_ki, 461 m_maximumIntegral / m_ki); 462 } 463 464 return m_kp * m_error + m_ki * m_totalError + m_kd * m_errorDerivative; 465 } 466 467 /** Resets the previous error and the integral term. */ 468 public void reset() { 469 m_error = 0; 470 m_prevError = 0; 471 m_totalError = 0; 472 m_errorDerivative = 0; 473 m_haveMeasurement = false; 474 } 475 476 @Override 477 public void initSendable(SendableBuilder builder) { 478 builder.setSmartDashboardType("PIDController"); 479 builder.addDoubleProperty("p", this::getP, this::setP); 480 builder.addDoubleProperty("i", this::getI, this::setI); 481 builder.addDoubleProperty("d", this::getD, this::setD); 482 builder.addDoubleProperty( 483 "izone", 484 this::getIZone, 485 (double toSet) -> { 486 try { 487 setIZone(toSet); 488 } catch (IllegalArgumentException e) { 489 MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace()); 490 } 491 }); 492 builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint); 493 builder.addDoubleProperty("measurement", () -> m_measurement, null); 494 builder.addDoubleProperty("error", this::getError, null); 495 builder.addDoubleProperty("error derivative", this::getErrorDerivative, null); 496 builder.addDoubleProperty("previous error", () -> this.m_prevError, null); 497 builder.addDoubleProperty("total error", this::getAccumulatedError, null); 498 } 499}