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