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