Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[wpimath] Don't recreate TrapezoidProfile in ProfiledPIDController calculate() #5863

Merged
merged 1 commit into from
Nov 3, 2023
Merged

[wpimath] Don't recreate TrapezoidProfile in ProfiledPIDController calculate() #5863

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 6 additions & 3 deletions wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,11 @@ public class ProfiledPIDController implements Sendable {
private PIDController m_controller;
private double m_minimumInput;
private double m_maximumInput;

private TrapezoidProfile.Constraints m_constraints;
private TrapezoidProfile m_profile;
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.
Expand Down Expand Up @@ -52,6 +54,7 @@ public ProfiledPIDController(
double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
m_controller = new PIDController(Kp, Ki, Kd, period);
m_constraints = constraints;
m_profile = new TrapezoidProfile(m_constraints);
instances++;

SendableRegistry.add(this, "ProfiledPIDController", instances);
Expand Down Expand Up @@ -219,6 +222,7 @@ public boolean atGoal() {
*/
public void setConstraints(TrapezoidProfile.Constraints constraints) {
m_constraints = constraints;
m_profile = new TrapezoidProfile(m_constraints);
}

/**
Expand Down Expand Up @@ -343,8 +347,7 @@ public double calculate(double measurement) {
m_setpoint.position = setpointMinDistance + measurement;
}

var profile = new TrapezoidProfile(m_constraints);
m_setpoint = profile.calculate(getPeriod(), m_goal, m_setpoint);
m_setpoint = m_profile.calculate(getPeriod(), m_goal, m_setpoint);
return m_controller.calculate(measurement, m_setpoint.position);
}

Expand Down
16 changes: 11 additions & 5 deletions wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,9 @@ class ProfiledPIDController
*/
ProfiledPIDController(double Kp, double Ki, double Kd,
Constraints constraints, units::second_t period = 20_ms)
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
: m_controller{Kp, Ki, Kd, period},
m_constraints{constraints},
m_profile{m_constraints} {
int instances = detail::IncrementAndGetProfiledPIDControllerInstances();
wpi::math::MathSharedStore::ReportUsage(
wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
Expand Down Expand Up @@ -202,7 +204,10 @@ class ProfiledPIDController
*
* @param constraints Velocity and acceleration constraints for goal.
*/
void SetConstraints(Constraints constraints) { m_constraints = constraints; }
void SetConstraints(Constraints constraints) {
m_constraints = constraints;
m_profile = TrapezoidProfile<Distance>{m_constraints};
}

/**
* Get the velocity and acceleration constraints for this controller.
Expand Down Expand Up @@ -317,8 +322,7 @@ class ProfiledPIDController
m_setpoint.position = setpointMinDistance + measurement;
}

frc::TrapezoidProfile<Distance> profile{m_constraints};
m_setpoint = profile.Calculate(GetPeriod(), m_goal, m_setpoint);
m_setpoint = m_profile.Calculate(GetPeriod(), m_goal, m_setpoint);
return m_controller.Calculate(measurement.value(),
m_setpoint.position.value());
}
Expand Down Expand Up @@ -408,9 +412,11 @@ class ProfiledPIDController
PIDController m_controller;
Distance_t m_minimumInput{0};
Distance_t m_maximumInput{0};

typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
TrapezoidProfile<Distance> m_profile;
typename frc::TrapezoidProfile<Distance>::State m_goal;
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
};

} // namespace frc