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