Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Tyler Veness
2023-11-03 15:21:58 -07:00
committed by GitHub
parent 8a3a268ae6
commit 6b53ef47cf
2 changed files with 17 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
View File

@@ -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.
@@ -52,6 +54,7 @@ public class ProfiledPIDController implements Sendable {
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);
@@ -219,6 +222,7 @@ public class ProfiledPIDController implements Sendable {
*/
public void setConstraints(TrapezoidProfile.Constraints constraints) {
m_constraints = constraints;
m_profile = new TrapezoidProfile(m_constraints);
}
/**
@@ -343,8 +347,7 @@ public class ProfiledPIDController implements Sendable {
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);
}

16
wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
View File

@@ -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);
@@ -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.
@@ -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());
}
@@ -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