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