001/* 002 * Copyright (C) Cross The Road Electronics. All rights reserved. 003 * License information can be found in CTRE_LICENSE.txt 004 * For support and suggestions contact support@ctr-electronics.com or file 005 * an issue tracker at https://github.com/CrossTheRoadElec/Phoenix-Releases 006 */ 007package com.ctre.phoenix6.mechanisms.swerve.utility; 008 009import edu.wpi.first.math.MathSharedStore; 010import edu.wpi.first.math.MathUsageId; 011import edu.wpi.first.math.MathUtil; 012import edu.wpi.first.util.sendable.Sendable; 013import edu.wpi.first.util.sendable.SendableBuilder; 014import edu.wpi.first.util.sendable.SendableRegistry; 015 016/** 017 * Phoenix-centric PID controller taken from WPI's {@link edu.wpi.first.math.controller.PIDController} class. 018 * <p> 019 * This class differs from the WPI implementation by using explicit timestamps for 020 * integral/derivative calculations. Ideally, these timestamps come from the {@link com.ctre.phoenix6.StatusSignal}. 021 */ 022public class PhoenixPIDController implements Sendable, AutoCloseable { 023 private static int instances; 024 025 // Factor for "proportional" control 026 private double m_kp; 027 028 // Factor for "integral" control 029 private double m_ki; 030 031 // Factor for "derivative" control 032 private double m_kd; 033 034 private double m_maximumIntegral = 1.0; 035 036 private double m_minimumIntegral = -1.0; 037 038 private double m_maximumInput; 039 040 private double m_minimumInput; 041 042 // Do the endpoints wrap around? e.g. Absolute encoder 043 private boolean m_continuous; 044 045 // The error at the time of the most recent call to calculate() 046 private double m_positionError; 047 private double m_velocityError; 048 049 // The error at the time of the second-most-recent call to calculate() (used to compute velocity) 050 private double m_prevError; 051 052 // The sum of the errors for use in the integral calc 053 private double m_totalError; 054 055 // The error that is considered at setpoint. 056 private double m_positionTolerance = 0.05; 057 private double m_velocityTolerance = Double.POSITIVE_INFINITY; 058 059 private double m_setpoint; 060 private double m_measurement; 061 062 private boolean m_haveMeasurement; 063 private boolean m_haveSetpoint; 064 065 private double m_lastAppliedOutput; 066 067 // The last timestamp acquired when performing a calculation 068 private double m_lastTimestamp; 069 070 /** 071 * Allocates a PIDController with the given constants for kp, ki, and kd. 072 * 073 * @param kp The proportional coefficient. 074 * @param ki The integral coefficient. 075 * @param kd The derivative coefficient. 076 */ 077 public PhoenixPIDController(double kp, double ki, double kd) { 078 m_kp = kp; 079 m_ki = ki; 080 m_kd = kd; 081 082 instances++; 083 SendableRegistry.addLW(this, "PIDController", instances); 084 085 MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances); 086 } 087 088 @Override 089 public void close() { 090 SendableRegistry.remove(this); 091 } 092 093 /** 094 * Sets the PID Controller gain parameters. 095 * 096 * <p>Set the proportional, integral, and differential coefficients. 097 * 098 * @param kp The proportional coefficient. 099 * @param ki The integral coefficient. 100 * @param kd The derivative coefficient. 101 */ 102 public void setPID(double kp, double ki, double kd) { 103 m_kp = kp; 104 m_ki = ki; 105 m_kd = kd; 106 } 107 108 /** 109 * Sets the Proportional coefficient of the PID controller gain. 110 * 111 * @param kp proportional coefficient 112 */ 113 public void setP(double kp) { 114 m_kp = kp; 115 } 116 117 /** 118 * Sets the Integral coefficient of the PID controller gain. 119 * 120 * @param ki integral coefficient 121 */ 122 public void setI(double ki) { 123 m_ki = ki; 124 } 125 126 /** 127 * Sets the Differential coefficient of the PID controller gain. 128 * 129 * @param kd differential coefficient 130 */ 131 public void setD(double kd) { 132 m_kd = kd; 133 } 134 135 /** 136 * Get the Proportional coefficient. 137 * 138 * @return proportional coefficient 139 */ 140 public double getP() { 141 return m_kp; 142 } 143 144 /** 145 * Get the Integral coefficient. 146 * 147 * @return integral coefficient 148 */ 149 public double getI() { 150 return m_ki; 151 } 152 153 /** 154 * Get the Differential coefficient. 155 * 156 * @return differential coefficient 157 */ 158 public double getD() { 159 return m_kd; 160 } 161 162 /** 163 * Returns the position tolerance of this controller. 164 * 165 * @return the position tolerance of the controller. 166 */ 167 public double getPositionTolerance() { 168 return m_positionTolerance; 169 } 170 171 /** 172 * Returns the velocity tolerance of this controller. 173 * 174 * @return the velocity tolerance of the controller. 175 */ 176 public double getVelocityTolerance() { 177 return m_velocityTolerance; 178 } 179 180 /** 181 * Returns the current setpoint of the PIDController. 182 * 183 * @return The current setpoint. 184 */ 185 public double getSetpoint() { 186 return m_setpoint; 187 } 188 189 /** 190 * Returns true if the error is within the tolerance of the setpoint. 191 * 192 * <p>This will return false until at least one input value has been computed. 193 * 194 * @return Whether the error is within the acceptable bounds. 195 */ 196 public boolean atSetpoint() { 197 return m_haveMeasurement 198 && m_haveSetpoint 199 && Math.abs(m_positionError) < m_positionTolerance 200 && Math.abs(m_velocityError) < m_velocityTolerance; 201 } 202 203 /** 204 * Enables continuous input. 205 * 206 * <p>Rather then using the max and min input range as constraints, it considers them to be the 207 * same point and automatically calculates the shortest route to the setpoint. 208 * 209 * @param minimumInput The minimum value expected from the input. 210 * @param maximumInput The maximum value expected from the input. 211 */ 212 public void enableContinuousInput(double minimumInput, double maximumInput) { 213 m_continuous = true; 214 m_minimumInput = minimumInput; 215 m_maximumInput = maximumInput; 216 } 217 218 /** Disables continuous input. */ 219 public void disableContinuousInput() { 220 m_continuous = false; 221 } 222 223 /** 224 * Returns true if continuous input is enabled. 225 * 226 * @return True if continuous input is enabled. 227 */ 228 public boolean isContinuousInputEnabled() { 229 return m_continuous; 230 } 231 232 /** 233 * Sets the minimum and maximum values for the integrator. 234 * 235 * <p>When the cap is reached, the integrator value is added to the controller output rather than 236 * the integrator value times the integral gain. 237 * 238 * @param minimumIntegral The minimum value of the integrator. 239 * @param maximumIntegral The maximum value of the integrator. 240 */ 241 public void setIntegratorRange(double minimumIntegral, double maximumIntegral) { 242 m_minimumIntegral = minimumIntegral; 243 m_maximumIntegral = maximumIntegral; 244 } 245 246 /** 247 * Sets the error which is considered tolerable for use with atSetpoint(). 248 * 249 * @param positionTolerance Position error which is tolerable. 250 */ 251 public void setTolerance(double positionTolerance) { 252 setTolerance(positionTolerance, Double.POSITIVE_INFINITY); 253 } 254 255 /** 256 * Sets the error which is considered tolerable for use with atSetpoint(). 257 * 258 * @param positionTolerance Position error which is tolerable. 259 * @param velocityTolerance Velocity error which is tolerable. 260 */ 261 public void setTolerance(double positionTolerance, double velocityTolerance) { 262 m_positionTolerance = positionTolerance; 263 m_velocityTolerance = velocityTolerance; 264 } 265 266 /** 267 * Returns the difference between the setpoint and the measurement. 268 * 269 * @return The error. 270 */ 271 public double getPositionError() { 272 return m_positionError; 273 } 274 275 /** 276 * Returns the velocity error. 277 * 278 * @return The velocity error. 279 */ 280 public double getVelocityError() { 281 return m_velocityError; 282 } 283 284 /** 285 * Returns the next output of the PID controller. 286 * 287 * @param measurement The current measurement of the process variable. 288 * @param setpoint The setpoint to target 289 * @param currentTimestamp The current timestamp to use for calculating integral/derivative error 290 * @return The next controller output. 291 */ 292 public double calculate(double measurement, double setpoint, double currentTimestamp) { 293 m_setpoint = setpoint; 294 m_haveSetpoint = true; 295 m_measurement = measurement; 296 m_prevError = m_positionError; 297 m_haveMeasurement = true; 298 299 double thisPeriod = currentTimestamp - m_lastTimestamp; 300 m_lastTimestamp = currentTimestamp; 301 302 if (m_continuous) { 303 double errorBound = (m_maximumInput - m_minimumInput) / 2.0; 304 m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound); 305 } else { 306 m_positionError = m_setpoint - m_measurement; 307 } 308 309 m_velocityError = (m_positionError - m_prevError) / thisPeriod; 310 311 if (m_ki != 0) { 312 m_totalError = 313 MathUtil.clamp( 314 m_totalError + m_positionError * thisPeriod, 315 m_minimumIntegral / m_ki, 316 m_maximumIntegral / m_ki); 317 } 318 319 m_lastAppliedOutput = m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError; 320 return m_lastAppliedOutput; 321 } 322 323 /* Get the last applied output from this PID controller */ 324 public double getLastAppliedOutput() { 325 return m_lastAppliedOutput; 326 } 327 328 /** Resets the previous error and the integral term. */ 329 public void reset() { 330 m_positionError = 0; 331 m_prevError = 0; 332 m_totalError = 0; 333 m_velocityError = 0; 334 m_haveMeasurement = false; 335 } 336 337 @Override 338 public void initSendable(SendableBuilder builder) { 339 builder.setSmartDashboardType("PIDController"); 340 builder.addDoubleProperty("p", this::getP, this::setP); 341 builder.addDoubleProperty("i", this::getI, this::setI); 342 builder.addDoubleProperty("d", this::getD, this::setD); 343 builder.addDoubleProperty("setpoint", this::getSetpoint, null); 344 } 345}