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.math.trajectory.TrapezoidProfile; 010import edu.wpi.first.util.sendable.Sendable; 011import edu.wpi.first.util.sendable.SendableBuilder; 012import edu.wpi.first.util.sendable.SendableRegistry; 013 014/** 015 * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should 016 * call reset() when they first start running the controller to avoid unwanted behavior. 017 */ 018public class ProfiledPIDController implements Sendable { 019 private static int instances; 020 021 private PIDController m_controller; 022 private double m_minimumInput; 023 private double m_maximumInput; 024 025 private TrapezoidProfile.Constraints m_constraints; 026 private TrapezoidProfile m_profile; 027 private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); 028 private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); 029 030 /** 031 * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd. 032 * 033 * @param Kp The proportional coefficient. 034 * @param Ki The integral coefficient. 035 * @param Kd The derivative coefficient. 036 * @param constraints Velocity and acceleration constraints for goal. 037 * @throws IllegalArgumentException if kp < 0 038 * @throws IllegalArgumentException if ki < 0 039 * @throws IllegalArgumentException if kd < 0 040 */ 041 public ProfiledPIDController( 042 double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) { 043 this(Kp, Ki, Kd, constraints, 0.02); 044 } 045 046 /** 047 * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd. 048 * 049 * @param Kp The proportional coefficient. 050 * @param Ki The integral coefficient. 051 * @param Kd The derivative coefficient. 052 * @param constraints Velocity and acceleration constraints for goal. 053 * @param period The period between controller updates in seconds. The default is 0.02 seconds. 054 * @throws IllegalArgumentException if kp < 0 055 * @throws IllegalArgumentException if ki < 0 056 * @throws IllegalArgumentException if kd < 0 057 * @throws IllegalArgumentException if period <= 0 058 */ 059 @SuppressWarnings("this-escape") 060 public ProfiledPIDController( 061 double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) { 062 m_controller = new PIDController(Kp, Ki, Kd, period); 063 m_constraints = constraints; 064 m_profile = new TrapezoidProfile(m_constraints); 065 instances++; 066 067 SendableRegistry.add(this, "ProfiledPIDController", instances); 068 MathSharedStore.reportUsage("ProfiledPIDController", String.valueOf(instances)); 069 } 070 071 /** 072 * Sets the PID Controller gain parameters. 073 * 074 * <p>Sets the proportional, integral, and differential coefficients. 075 * 076 * @param Kp The proportional coefficient. Must be >= 0. 077 * @param Ki The integral coefficient. Must be >= 0. 078 * @param Kd The differential coefficient. Must be >= 0. 079 */ 080 public void setPID(double Kp, double Ki, double Kd) { 081 m_controller.setPID(Kp, Ki, Kd); 082 } 083 084 /** 085 * Sets the proportional coefficient of the PID controller gain. 086 * 087 * @param Kp The proportional coefficient. Must be >= 0. 088 */ 089 public void setP(double Kp) { 090 m_controller.setP(Kp); 091 } 092 093 /** 094 * Sets the integral coefficient of the PID controller gain. 095 * 096 * @param Ki The integral coefficient. Must be >= 0. 097 */ 098 public void setI(double Ki) { 099 m_controller.setI(Ki); 100 } 101 102 /** 103 * Sets the differential coefficient of the PID controller gain. 104 * 105 * @param Kd The differential coefficient. Must be >= 0. 106 */ 107 public void setD(double Kd) { 108 m_controller.setD(Kd); 109 } 110 111 /** 112 * Sets the IZone range. When the absolute value of the position error is greater than IZone, the 113 * total accumulated error will reset to zero, disabling integral gain until the absolute value of 114 * the position error is less than IZone. This is used to prevent integral windup. Must be 115 * non-negative. Passing a value of zero will effectively disable integral gain. Passing a value 116 * of {@link Double#POSITIVE_INFINITY} disables IZone functionality. 117 * 118 * @param iZone Maximum magnitude of error to allow integral control. 119 * @throws IllegalArgumentException if iZone <= 0 120 */ 121 public void setIZone(double iZone) { 122 m_controller.setIZone(iZone); 123 } 124 125 /** 126 * Gets the proportional coefficient. 127 * 128 * @return proportional coefficient 129 */ 130 public double getP() { 131 return m_controller.getP(); 132 } 133 134 /** 135 * Gets the integral coefficient. 136 * 137 * @return integral coefficient 138 */ 139 public double getI() { 140 return m_controller.getI(); 141 } 142 143 /** 144 * Gets the differential coefficient. 145 * 146 * @return differential coefficient 147 */ 148 public double getD() { 149 return m_controller.getD(); 150 } 151 152 /** 153 * Get the IZone range. 154 * 155 * @return Maximum magnitude of error to allow integral control. 156 */ 157 public double getIZone() { 158 return m_controller.getIZone(); 159 } 160 161 /** 162 * Gets the period of this controller. 163 * 164 * @return The period of the controller. 165 */ 166 public double getPeriod() { 167 return m_controller.getPeriod(); 168 } 169 170 /** 171 * Returns the position tolerance of this controller. 172 * 173 * @return the position tolerance of the controller. 174 */ 175 public double getPositionTolerance() { 176 return m_controller.getErrorTolerance(); 177 } 178 179 /** 180 * Returns the velocity tolerance of this controller. 181 * 182 * @return the velocity tolerance of the controller. 183 */ 184 public double getVelocityTolerance() { 185 return m_controller.getErrorDerivativeTolerance(); 186 } 187 188 /** 189 * Returns the accumulated error used in the integral calculation of this controller. 190 * 191 * @return The accumulated error of this controller. 192 */ 193 public double getAccumulatedError() { 194 return m_controller.getAccumulatedError(); 195 } 196 197 /** 198 * Sets the goal for the ProfiledPIDController. 199 * 200 * @param goal The desired goal state. 201 */ 202 public void setGoal(TrapezoidProfile.State goal) { 203 m_goal = goal; 204 } 205 206 /** 207 * Sets the goal for the ProfiledPIDController. 208 * 209 * @param goal The desired goal position. 210 */ 211 public void setGoal(double goal) { 212 m_goal = new TrapezoidProfile.State(goal, 0); 213 } 214 215 /** 216 * Gets the goal for the ProfiledPIDController. 217 * 218 * @return The goal. 219 */ 220 public TrapezoidProfile.State getGoal() { 221 return m_goal; 222 } 223 224 /** 225 * Returns true if the error is within the tolerance of the error. 226 * 227 * <p>This will return false until at least one input value has been computed. 228 * 229 * @return True if the error is within the tolerance of the error. 230 */ 231 public boolean atGoal() { 232 return atSetpoint() && m_goal.equals(m_setpoint); 233 } 234 235 /** 236 * Set velocity and acceleration constraints for goal. 237 * 238 * @param constraints Velocity and acceleration constraints for goal. 239 */ 240 public void setConstraints(TrapezoidProfile.Constraints constraints) { 241 m_constraints = constraints; 242 m_profile = new TrapezoidProfile(m_constraints); 243 } 244 245 /** 246 * Get the velocity and acceleration constraints for this controller. 247 * 248 * @return Velocity and acceleration constraints. 249 */ 250 public TrapezoidProfile.Constraints getConstraints() { 251 return m_constraints; 252 } 253 254 /** 255 * Returns the current setpoint of the ProfiledPIDController. 256 * 257 * @return The current setpoint. 258 */ 259 public TrapezoidProfile.State getSetpoint() { 260 return m_setpoint; 261 } 262 263 /** 264 * Returns true if the error is within the tolerance of the error. 265 * 266 * <p>This will return false until at least one input value has been computed. 267 * 268 * @return True if the error is within the tolerance of the error. 269 */ 270 public boolean atSetpoint() { 271 return m_controller.atSetpoint(); 272 } 273 274 /** 275 * Enables continuous input. 276 * 277 * <p>Rather then using the max and min input range as constraints, it considers them to be the 278 * same point and automatically calculates the shortest route to the setpoint. 279 * 280 * @param minimumInput The minimum value expected from the input. 281 * @param maximumInput The maximum value expected from the input. 282 */ 283 public void enableContinuousInput(double minimumInput, double maximumInput) { 284 m_controller.enableContinuousInput(minimumInput, maximumInput); 285 m_minimumInput = minimumInput; 286 m_maximumInput = maximumInput; 287 } 288 289 /** Disables continuous input. */ 290 public void disableContinuousInput() { 291 m_controller.disableContinuousInput(); 292 } 293 294 /** 295 * Sets the minimum and maximum contributions of the integral term. 296 * 297 * <p>The internal integrator is clamped so that the integral term's contribution to the output 298 * stays between minimumIntegral and maximumIntegral. This prevents integral windup. 299 * 300 * @param minimumIntegral The minimum contribution of the integral term. 301 * @param maximumIntegral The maximum contribution of the integral term. 302 */ 303 public void setIntegratorRange(double minimumIntegral, double maximumIntegral) { 304 m_controller.setIntegratorRange(minimumIntegral, maximumIntegral); 305 } 306 307 /** 308 * Sets the error which is considered tolerable for use with atSetpoint(). 309 * 310 * @param positionTolerance Position error which is tolerable. 311 */ 312 public void setTolerance(double positionTolerance) { 313 setTolerance(positionTolerance, Double.POSITIVE_INFINITY); 314 } 315 316 /** 317 * Sets the error which is considered tolerable for use with atSetpoint(). 318 * 319 * @param positionTolerance Position error which is tolerable. 320 * @param velocityTolerance Velocity error which is tolerable. 321 */ 322 public void setTolerance(double positionTolerance, double velocityTolerance) { 323 m_controller.setTolerance(positionTolerance, velocityTolerance); 324 } 325 326 /** 327 * Returns the difference between the setpoint and the measurement. 328 * 329 * @return The error. 330 */ 331 public double getPositionError() { 332 return m_controller.getError(); 333 } 334 335 /** 336 * Returns the change in error per second. 337 * 338 * @return The change in error per second. 339 */ 340 public double getVelocityError() { 341 return m_controller.getErrorDerivative(); 342 } 343 344 /** 345 * Returns the next output of the PID controller. 346 * 347 * @param measurement The current measurement of the process variable. 348 * @return The controller's next output. 349 */ 350 public double calculate(double measurement) { 351 if (m_controller.isContinuousInputEnabled()) { 352 // Get error which is the smallest distance between goal and measurement 353 double errorBound = (m_maximumInput - m_minimumInput) / 2.0; 354 double goalMinDistance = 355 MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound); 356 double setpointMinDistance = 357 MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound); 358 359 // Recompute the profile goal with the smallest error, thus giving the shortest path. The goal 360 // may be outside the input range after this operation, but that's OK because the controller 361 // will still go there and report an error of zero. In other words, the setpoint only needs to 362 // be offset from the measurement by the input range modulus; they don't need to be equal. 363 m_goal.position = goalMinDistance + measurement; 364 m_setpoint.position = setpointMinDistance + measurement; 365 } 366 367 m_setpoint = m_profile.calculate(getPeriod(), m_setpoint, m_goal); 368 return m_controller.calculate(measurement, m_setpoint.position); 369 } 370 371 /** 372 * Returns the next output of the PID controller. 373 * 374 * @param measurement The current measurement of the process variable. 375 * @param goal The new goal of the controller. 376 * @return The controller's next output. 377 */ 378 public double calculate(double measurement, TrapezoidProfile.State goal) { 379 setGoal(goal); 380 return calculate(measurement); 381 } 382 383 /** 384 * Returns the next output of the PIDController. 385 * 386 * @param measurement The current measurement of the process variable. 387 * @param goal The new goal of the controller. 388 * @return The controller's next output. 389 */ 390 public double calculate(double measurement, double goal) { 391 setGoal(goal); 392 return calculate(measurement); 393 } 394 395 /** 396 * Returns the next output of the PID controller. 397 * 398 * @param measurement The current measurement of the process variable. 399 * @param goal The new goal of the controller. 400 * @param constraints Velocity and acceleration constraints for goal. 401 * @return The controller's next output. 402 */ 403 public double calculate( 404 double measurement, TrapezoidProfile.State goal, TrapezoidProfile.Constraints constraints) { 405 setConstraints(constraints); 406 return calculate(measurement, goal); 407 } 408 409 /** 410 * Reset the previous error and the integral term. 411 * 412 * @param measurement The current measured State of the system. 413 */ 414 public void reset(TrapezoidProfile.State measurement) { 415 m_controller.reset(); 416 m_setpoint = measurement; 417 } 418 419 /** 420 * Reset the previous error and the integral term. 421 * 422 * @param measuredPosition The current measured position of the system. 423 * @param measuredVelocity The current measured velocity of the system. 424 */ 425 public void reset(double measuredPosition, double measuredVelocity) { 426 reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity)); 427 } 428 429 /** 430 * Reset the previous error and the integral term. 431 * 432 * @param measuredPosition The current measured position of the system. The velocity is assumed to 433 * be zero. 434 */ 435 public void reset(double measuredPosition) { 436 reset(measuredPosition, 0.0); 437 } 438 439 @Override 440 public void initSendable(SendableBuilder builder) { 441 builder.setSmartDashboardType("ProfiledPIDController"); 442 builder.addDoubleProperty("p", this::getP, this::setP); 443 builder.addDoubleProperty("i", this::getI, this::setI); 444 builder.addDoubleProperty("d", this::getD, this::setD); 445 builder.addDoubleProperty( 446 "izone", 447 this::getIZone, 448 (double toSet) -> { 449 try { 450 setIZone(toSet); 451 } catch (IllegalArgumentException e) { 452 MathSharedStore.reportError("IZone must be a non-negative number!", e.getStackTrace()); 453 } 454 }); 455 builder.addDoubleProperty( 456 "maxVelocity", 457 () -> getConstraints().maxVelocity, 458 maxVelocity -> 459 setConstraints( 460 new TrapezoidProfile.Constraints(maxVelocity, getConstraints().maxAcceleration))); 461 builder.addDoubleProperty( 462 "maxAcceleration", 463 () -> getConstraints().maxAcceleration, 464 maxAcceleration -> 465 setConstraints( 466 new TrapezoidProfile.Constraints(getConstraints().maxVelocity, maxAcceleration))); 467 builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal); 468 } 469}