mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Move controller from wpilibj to wpimath (#3439)
This commit is contained in:
Noam Zaks
@@ -12,5 +12,7 @@ public enum MathUsageId {
|
||||
kFilter_Linear,
|
||||
kOdometry_DifferentialDrive,
|
||||
kOdometry_SwerveDrive,
|
||||
kOdometry_MecanumDrive
|
||||
kOdometry_MecanumDrive,
|
||||
kController_PIDController2,
|
||||
kController_ProfiledPIDController,
|
||||
}
|
||||
|
||||
@@ -0,0 +1,139 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.trajectory.Trajectory;
|
||||
|
||||
/**
|
||||
* This holonomic drive controller can be used to follow trajectories using a holonomic drive train
|
||||
* (i.e. swerve or mecanum). Holonomic trajectory following is a much simpler problem to solve
|
||||
* compared to skid-steer style drivetrains because it is possible to individually control forward,
|
||||
* sideways, and angular velocity.
|
||||
*
|
||||
* <p>The holonomic drive controller takes in one PID controller for each direction, forward and
|
||||
* sideways, and one profiled PID controller for the angular direction. Because the heading dynamics
|
||||
* are decoupled from translations, users can specify a custom heading that the drivetrain should
|
||||
* point toward. This heading reference is profiled for smoothness.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class HolonomicDriveController {
|
||||
private Pose2d m_poseError = new Pose2d();
|
||||
private Rotation2d m_rotationError = new Rotation2d();
|
||||
private Pose2d m_poseTolerance = new Pose2d();
|
||||
private boolean m_enabled = true;
|
||||
|
||||
private final PIDController m_xController;
|
||||
private final PIDController m_yController;
|
||||
private final ProfiledPIDController m_thetaController;
|
||||
|
||||
private boolean m_firstRun = true;
|
||||
|
||||
/**
|
||||
* Constructs a holonomic drive controller.
|
||||
*
|
||||
* @param xController A PID Controller to respond to error in the field-relative x direction.
|
||||
* @param yController A PID Controller to respond to error in the field-relative y direction.
|
||||
* @param thetaController A profiled PID controller to respond to error in angle.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public HolonomicDriveController(
|
||||
PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
|
||||
m_xController = xController;
|
||||
m_yController = yController;
|
||||
m_thetaController = thetaController;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*
|
||||
* @return True if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
public boolean atReference() {
|
||||
final var eTranslate = m_poseError.getTranslation();
|
||||
final var eRotate = m_rotationError;
|
||||
final var tolTranslate = m_poseTolerance.getTranslation();
|
||||
final var tolRotate = m_poseTolerance.getRotation();
|
||||
return Math.abs(eTranslate.getX()) < tolTranslate.getX()
|
||||
&& Math.abs(eTranslate.getY()) < tolTranslate.getY()
|
||||
&& Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerance for use with atReference().
|
||||
*
|
||||
* @param tolerance The pose error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(Pose2d tolerance) {
|
||||
m_poseTolerance = tolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param poseRef The desired pose.
|
||||
* @param linearVelocityRefMeters The linear velocity reference.
|
||||
* @param angleRef The angular reference.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
|
||||
// If this is the first run, then we need to reset the theta controller to the current pose's
|
||||
// heading.
|
||||
if (m_firstRun) {
|
||||
m_thetaController.reset(currentPose.getRotation().getRadians());
|
||||
m_firstRun = false;
|
||||
}
|
||||
|
||||
// Calculate feedforward velocities (field-relative).
|
||||
double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
|
||||
double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
|
||||
double thetaFF =
|
||||
m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
|
||||
|
||||
m_poseError = poseRef.relativeTo(currentPose);
|
||||
m_rotationError = angleRef.minus(currentPose.getRotation());
|
||||
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
|
||||
}
|
||||
|
||||
// Calculate feedback velocities (based on position error).
|
||||
double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
|
||||
double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
|
||||
|
||||
// Return next output.
|
||||
return ChassisSpeeds.fromFieldRelativeSpeeds(
|
||||
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param desiredState The desired trajectory state.
|
||||
* @param angleRef The desired end-angle.
|
||||
* @return The next output of the holonomic drive controller.
|
||||
*/
|
||||
public ChassisSpeeds calculate(
|
||||
Pose2d currentPose, Trajectory.State desiredState, Rotation2d angleRef) {
|
||||
return calculate(
|
||||
currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, angleRef);
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting problems. When calculate() is called on
|
||||
* a disabled controller, only feedforward values are returned.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
public void setEnabled(boolean enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,355 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.MathSharedStore;
|
||||
import edu.wpi.first.math.MathUsageId;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
import edu.wpi.first.util.sendable.SendableRegistry;
|
||||
|
||||
/** Implements a PID control loop. */
|
||||
public class PIDController implements Sendable, AutoCloseable {
|
||||
private static int instances;
|
||||
|
||||
// Factor for "proportional" control
|
||||
private double m_kp;
|
||||
|
||||
// Factor for "integral" control
|
||||
private double m_ki;
|
||||
|
||||
// Factor for "derivative" control
|
||||
private double m_kd;
|
||||
|
||||
// The period (in seconds) of the loop that calls the controller
|
||||
private final double m_period;
|
||||
|
||||
private double m_maximumIntegral = 1.0;
|
||||
|
||||
private double m_minimumIntegral = -1.0;
|
||||
|
||||
private double m_maximumInput;
|
||||
|
||||
private double m_minimumInput;
|
||||
|
||||
// Do the endpoints wrap around? eg. Absolute encoder
|
||||
private boolean m_continuous;
|
||||
|
||||
// The error at the time of the most recent call to calculate()
|
||||
private double m_positionError;
|
||||
private double m_velocityError;
|
||||
|
||||
// The error at the time of the second-most-recent call to calculate() (used to compute velocity)
|
||||
private double m_prevError;
|
||||
|
||||
// The sum of the errors for use in the integral calc
|
||||
private double m_totalError;
|
||||
|
||||
// The error that is considered at setpoint.
|
||||
private double m_positionTolerance = 0.05;
|
||||
private double m_velocityTolerance = Double.POSITIVE_INFINITY;
|
||||
|
||||
private double m_setpoint;
|
||||
private double m_measurement;
|
||||
|
||||
/**
|
||||
* Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
|
||||
* 0.02 seconds.
|
||||
*
|
||||
* @param kp The proportional coefficient.
|
||||
* @param ki The integral coefficient.
|
||||
* @param kd The derivative coefficient.
|
||||
*/
|
||||
public PIDController(double kp, double ki, double kd) {
|
||||
this(kp, ki, kd, 0.02);
|
||||
}
|
||||
|
||||
/**
|
||||
* Allocates a PIDController with the given constants for kp, ki, and kd.
|
||||
*
|
||||
* @param kp The proportional coefficient.
|
||||
* @param ki The integral coefficient.
|
||||
* @param kd The derivative coefficient.
|
||||
* @param period The period between controller updates in seconds. Must be non-zero and positive.
|
||||
*/
|
||||
public PIDController(double kp, double ki, double kd, double period) {
|
||||
m_kp = kp;
|
||||
m_ki = ki;
|
||||
m_kd = kd;
|
||||
|
||||
if (period <= 0) {
|
||||
throw new IllegalArgumentException("Controller period must be a non-zero positive number!");
|
||||
}
|
||||
m_period = period;
|
||||
|
||||
instances++;
|
||||
SendableRegistry.addLW(this, "PIDController", instances);
|
||||
|
||||
MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void close() {
|
||||
SendableRegistry.remove(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
*
|
||||
* <p>Set the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param kp The proportional coefficient.
|
||||
* @param ki The integral coefficient.
|
||||
* @param kd The derivative coefficient.
|
||||
*/
|
||||
public void setPID(double kp, double ki, double kd) {
|
||||
m_kp = kp;
|
||||
m_ki = ki;
|
||||
m_kd = kd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the Proportional coefficient of the PID controller gain.
|
||||
*
|
||||
* @param kp proportional coefficient
|
||||
*/
|
||||
public void setP(double kp) {
|
||||
m_kp = kp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the Integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param ki integral coefficient
|
||||
*/
|
||||
public void setI(double ki) {
|
||||
m_ki = ki;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the Differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param kd differential coefficient
|
||||
*/
|
||||
public void setD(double kd) {
|
||||
m_kd = kd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the Proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
public double getP() {
|
||||
return m_kp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the Integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
public double getI() {
|
||||
return m_ki;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the Differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
public double getD() {
|
||||
return m_kd;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the period of this controller.
|
||||
*
|
||||
* @return the period of the controller.
|
||||
*/
|
||||
public double getPeriod() {
|
||||
return m_period;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the setpoint for the PIDController.
|
||||
*
|
||||
* @param setpoint The desired setpoint.
|
||||
*/
|
||||
public void setSetpoint(double setpoint) {
|
||||
m_setpoint = setpoint;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the PIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
public double getSetpoint() {
|
||||
return m_setpoint;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the setpoint.
|
||||
*
|
||||
* <p>This will return false until at least one input value has been computed.
|
||||
*
|
||||
* @return Whether the error is within the acceptable bounds.
|
||||
*/
|
||||
public boolean atSetpoint() {
|
||||
double positionError;
|
||||
if (m_continuous) {
|
||||
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
|
||||
} else {
|
||||
positionError = m_setpoint - m_measurement;
|
||||
}
|
||||
|
||||
double velocityError = (positionError - m_prevError) / m_period;
|
||||
|
||||
return Math.abs(positionError) < m_positionTolerance
|
||||
&& Math.abs(velocityError) < m_velocityTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
*
|
||||
* <p>Rather then using the max and min input range as constraints, it considers them to be the
|
||||
* same point and automatically calculates the shortest route to the setpoint.
|
||||
*
|
||||
* @param minimumInput The minimum value expected from the input.
|
||||
* @param maximumInput The maximum value expected from the input.
|
||||
*/
|
||||
public void enableContinuousInput(double minimumInput, double maximumInput) {
|
||||
m_continuous = true;
|
||||
m_minimumInput = minimumInput;
|
||||
m_maximumInput = maximumInput;
|
||||
}
|
||||
|
||||
/** Disables continuous input. */
|
||||
public void disableContinuousInput() {
|
||||
m_continuous = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if continuous input is enabled.
|
||||
*
|
||||
* @return True if continuous input is enabled.
|
||||
*/
|
||||
public boolean isContinuousInputEnabled() {
|
||||
return m_continuous;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values for the integrator.
|
||||
*
|
||||
* <p>When the cap is reached, the integrator value is added to the controller output rather than
|
||||
* the integrator value times the integral gain.
|
||||
*
|
||||
* @param minimumIntegral The minimum value of the integrator.
|
||||
* @param maximumIntegral The maximum value of the integrator.
|
||||
*/
|
||||
public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
|
||||
m_minimumIntegral = minimumIntegral;
|
||||
m_maximumIntegral = maximumIntegral;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the error which is considered tolerable for use with atSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(double positionTolerance) {
|
||||
setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the error which is considered tolerable for use with atSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
* @param velocityTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(double positionTolerance, double velocityTolerance) {
|
||||
m_positionTolerance = positionTolerance;
|
||||
m_velocityTolerance = velocityTolerance;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the difference between the setpoint and the measurement.
|
||||
*
|
||||
* @return The error.
|
||||
*/
|
||||
public double getPositionError() {
|
||||
return m_positionError;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the velocity error.
|
||||
*
|
||||
* @return The velocity error.
|
||||
*/
|
||||
public double getVelocityError() {
|
||||
return m_velocityError;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param setpoint The new setpoint of the controller.
|
||||
* @return The next controller output.
|
||||
*/
|
||||
public double calculate(double measurement, double setpoint) {
|
||||
// Set setpoint to provided value
|
||||
setSetpoint(setpoint);
|
||||
return calculate(measurement);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @return The next controller output.
|
||||
*/
|
||||
public double calculate(double measurement) {
|
||||
m_measurement = measurement;
|
||||
m_prevError = m_positionError;
|
||||
|
||||
if (m_continuous) {
|
||||
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
|
||||
} else {
|
||||
m_positionError = m_setpoint - measurement;
|
||||
}
|
||||
|
||||
m_velocityError = (m_positionError - m_prevError) / m_period;
|
||||
|
||||
if (m_ki != 0) {
|
||||
m_totalError =
|
||||
MathUtil.clamp(
|
||||
m_totalError + m_positionError * m_period,
|
||||
m_minimumIntegral / m_ki,
|
||||
m_maximumIntegral / m_ki);
|
||||
}
|
||||
|
||||
return m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError;
|
||||
}
|
||||
|
||||
/** Resets the previous error and the integral term. */
|
||||
public void reset() {
|
||||
m_prevError = 0;
|
||||
m_totalError = 0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("PIDController");
|
||||
builder.addDoubleProperty("p", this::getP, this::setP);
|
||||
builder.addDoubleProperty("i", this::getI, this::setI);
|
||||
builder.addDoubleProperty("d", this::getD, this::setD);
|
||||
builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,381 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.math.controller;
|
||||
|
||||
import edu.wpi.first.math.MathSharedStore;
|
||||
import edu.wpi.first.math.MathUsageId;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.util.sendable.Sendable;
|
||||
import edu.wpi.first.util.sendable.SendableBuilder;
|
||||
|
||||
/**
|
||||
* Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
|
||||
* call reset() when they first start running the controller to avoid unwanted behavior.
|
||||
*/
|
||||
public class ProfiledPIDController implements Sendable {
|
||||
private static int instances;
|
||||
|
||||
private PIDController m_controller;
|
||||
private double m_minimumInput;
|
||||
private double m_maximumInput;
|
||||
private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
|
||||
private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
|
||||
private TrapezoidProfile.Constraints m_constraints;
|
||||
|
||||
/**
|
||||
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
|
||||
*
|
||||
* @param Kp The proportional coefficient.
|
||||
* @param Ki The integral coefficient.
|
||||
* @param Kd The derivative coefficient.
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public ProfiledPIDController(
|
||||
double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
|
||||
this(Kp, Ki, Kd, constraints, 0.02);
|
||||
}
|
||||
|
||||
/**
|
||||
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
|
||||
*
|
||||
* @param Kp The proportional coefficient.
|
||||
* @param Ki The integral coefficient.
|
||||
* @param Kd The derivative coefficient.
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
* @param period The period between controller updates in seconds. The default is 0.02 seconds.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public ProfiledPIDController(
|
||||
double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
|
||||
m_controller = new PIDController(Kp, Ki, Kd, period);
|
||||
m_constraints = constraints;
|
||||
instances++;
|
||||
MathSharedStore.reportUsage(MathUsageId.kController_ProfiledPIDController, instances);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
*
|
||||
* <p>Sets the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param Kp Proportional coefficient
|
||||
* @param Ki Integral coefficient
|
||||
* @param Kd Differential coefficient
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void setPID(double Kp, double Ki, double Kd) {
|
||||
m_controller.setPID(Kp, Ki, Kd);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the proportional coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kp proportional coefficient
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void setP(double Kp) {
|
||||
m_controller.setP(Kp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Ki integral coefficient
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void setI(double Ki) {
|
||||
m_controller.setI(Ki);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kd differential coefficient
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public void setD(double Kd) {
|
||||
m_controller.setD(Kd);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
public double getP() {
|
||||
return m_controller.getP();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
public double getI() {
|
||||
return m_controller.getI();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
public double getD() {
|
||||
return m_controller.getD();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the period of this controller.
|
||||
*
|
||||
* @return The period of the controller.
|
||||
*/
|
||||
public double getPeriod() {
|
||||
return m_controller.getPeriod();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @param goal The desired goal state.
|
||||
*/
|
||||
public void setGoal(TrapezoidProfile.State goal) {
|
||||
m_goal = goal;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @param goal The desired goal position.
|
||||
*/
|
||||
public void setGoal(double goal) {
|
||||
m_goal = new TrapezoidProfile.State(goal, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @return The goal.
|
||||
*/
|
||||
public TrapezoidProfile.State getGoal() {
|
||||
return m_goal;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
*
|
||||
* <p>This will return false until at least one input value has been computed.
|
||||
*
|
||||
* @return True if the error is within the tolerance of the error.
|
||||
*/
|
||||
public boolean atGoal() {
|
||||
return atSetpoint() && m_goal.equals(m_setpoint);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set velocity and acceleration constraints for goal.
|
||||
*
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
public void setConstraints(TrapezoidProfile.Constraints constraints) {
|
||||
m_constraints = constraints;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the ProfiledPIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
public TrapezoidProfile.State getSetpoint() {
|
||||
return m_setpoint;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
*
|
||||
* <p>This will return false until at least one input value has been computed.
|
||||
*
|
||||
* @return True if the error is within the tolerance of the error.
|
||||
*/
|
||||
public boolean atSetpoint() {
|
||||
return m_controller.atSetpoint();
|
||||
}
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
*
|
||||
* <p>Rather then using the max and min input range as constraints, it considers them to be the
|
||||
* same point and automatically calculates the shortest route to the setpoint.
|
||||
*
|
||||
* @param minimumInput The minimum value expected from the input.
|
||||
* @param maximumInput The maximum value expected from the input.
|
||||
*/
|
||||
public void enableContinuousInput(double minimumInput, double maximumInput) {
|
||||
m_controller.enableContinuousInput(minimumInput, maximumInput);
|
||||
m_minimumInput = minimumInput;
|
||||
m_maximumInput = maximumInput;
|
||||
}
|
||||
|
||||
/** Disables continuous input. */
|
||||
public void disableContinuousInput() {
|
||||
m_controller.disableContinuousInput();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values for the integrator.
|
||||
*
|
||||
* <p>When the cap is reached, the integrator value is added to the controller output rather than
|
||||
* the integrator value times the integral gain.
|
||||
*
|
||||
* @param minimumIntegral The minimum value of the integrator.
|
||||
* @param maximumIntegral The maximum value of the integrator.
|
||||
*/
|
||||
public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
|
||||
m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the error which is considered tolerable for use with atSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(double positionTolerance) {
|
||||
setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the error which is considered tolerable for use with atSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
* @param velocityTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
public void setTolerance(double positionTolerance, double velocityTolerance) {
|
||||
m_controller.setTolerance(positionTolerance, velocityTolerance);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the difference between the setpoint and the measurement.
|
||||
*
|
||||
* @return The error.
|
||||
*/
|
||||
public double getPositionError() {
|
||||
return m_controller.getPositionError();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the change in error per second.
|
||||
*
|
||||
* @return The change in error per second.
|
||||
*/
|
||||
public double getVelocityError() {
|
||||
return m_controller.getVelocityError();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @return The controller's next output.
|
||||
*/
|
||||
public double calculate(double measurement) {
|
||||
if (m_controller.isContinuousInputEnabled()) {
|
||||
// Get error which is smallest distance between goal and measurement
|
||||
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
double goalMinDistance =
|
||||
MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
|
||||
double setpointMinDistance =
|
||||
MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound);
|
||||
|
||||
// Recompute the profile goal with the smallest error, thus giving the shortest path. The goal
|
||||
// may be outside the input range after this operation, but that's OK because the controller
|
||||
// will still go there and report an error of zero. In other words, the setpoint only needs to
|
||||
// be offset from the measurement by the input range modulus; they don't need to be equal.
|
||||
m_goal.position = goalMinDistance + measurement;
|
||||
m_setpoint.position = setpointMinDistance + measurement;
|
||||
}
|
||||
|
||||
var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
|
||||
m_setpoint = profile.calculate(getPeriod());
|
||||
return m_controller.calculate(measurement, m_setpoint.position);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param goal The new goal of the controller.
|
||||
* @return The controller's next output.
|
||||
*/
|
||||
public double calculate(double measurement, TrapezoidProfile.State goal) {
|
||||
setGoal(goal);
|
||||
return calculate(measurement);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PIDController.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param goal The new goal of the controller.
|
||||
* @return The controller's next output.
|
||||
*/
|
||||
public double calculate(double measurement, double goal) {
|
||||
setGoal(goal);
|
||||
return calculate(measurement);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
* @param goal The new goal of the controller.
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
* @return The controller's next output.
|
||||
*/
|
||||
public double calculate(
|
||||
double measurement, TrapezoidProfile.State goal, TrapezoidProfile.Constraints constraints) {
|
||||
setConstraints(constraints);
|
||||
return calculate(measurement, goal);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the previous error and the integral term.
|
||||
*
|
||||
* @param measurement The current measured State of the system.
|
||||
*/
|
||||
public void reset(TrapezoidProfile.State measurement) {
|
||||
m_controller.reset();
|
||||
m_setpoint = measurement;
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the previous error and the integral term.
|
||||
*
|
||||
* @param measuredPosition The current measured position of the system.
|
||||
* @param measuredVelocity The current measured velocity of the system.
|
||||
*/
|
||||
public void reset(double measuredPosition, double measuredVelocity) {
|
||||
reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the previous error and the integral term.
|
||||
*
|
||||
* @param measuredPosition The current measured position of the system. The velocity is assumed to
|
||||
* be zero.
|
||||
*/
|
||||
public void reset(double measuredPosition) {
|
||||
reset(measuredPosition, 0.0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void initSendable(SendableBuilder builder) {
|
||||
builder.setSmartDashboardType("ProfiledPIDController");
|
||||
builder.addDoubleProperty("p", this::getP, this::setP);
|
||||
builder.addDoubleProperty("i", this::getI, this::setI);
|
||||
builder.addDoubleProperty("d", this::getD, this::setD);
|
||||
builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user