mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-20 00:51:42 +00:00
Template C++ TrapezoidProfile and ProfiledPIDController on units (#2109)
This commit is contained in:
Oblarg
@@ -1,133 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/controller/ProfiledPIDController.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
#include "frc/smartdashboard/SendableBuilder.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
ProfiledPIDController::ProfiledPIDController(
|
||||
double Kp, double Ki, double Kd,
|
||||
frc::TrapezoidProfile::Constraints constraints, units::second_t period)
|
||||
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
|
||||
|
||||
void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
|
||||
|
||||
void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
|
||||
|
||||
void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
|
||||
|
||||
double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
|
||||
|
||||
double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
|
||||
|
||||
double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
|
||||
|
||||
units::second_t ProfiledPIDController::GetPeriod() const {
|
||||
return m_controller.GetPeriod();
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
|
||||
m_goal = goal;
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetGoal(units::meter_t goal) {
|
||||
m_goal = {goal, 0_mps};
|
||||
}
|
||||
|
||||
TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
|
||||
return m_goal;
|
||||
}
|
||||
|
||||
bool ProfiledPIDController::AtGoal() const {
|
||||
return AtSetpoint() && m_goal == m_setpoint;
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetConstraints(
|
||||
frc::TrapezoidProfile::Constraints constraints) {
|
||||
m_constraints = constraints;
|
||||
}
|
||||
|
||||
TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
|
||||
return m_setpoint;
|
||||
}
|
||||
|
||||
bool ProfiledPIDController::AtSetpoint() const {
|
||||
return m_controller.AtSetpoint();
|
||||
}
|
||||
|
||||
void ProfiledPIDController::EnableContinuousInput(double minimumInput,
|
||||
double maximumInput) {
|
||||
m_controller.EnableContinuousInput(minimumInput, maximumInput);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::DisableContinuousInput() {
|
||||
m_controller.DisableContinuousInput();
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetIntegratorRange(double minimumIntegral,
|
||||
double maximumIntegral) {
|
||||
m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::SetTolerance(double positionTolerance,
|
||||
double velocityTolerance) {
|
||||
m_controller.SetTolerance(positionTolerance, velocityTolerance);
|
||||
}
|
||||
|
||||
double ProfiledPIDController::GetPositionError() const {
|
||||
return m_controller.GetPositionError();
|
||||
}
|
||||
|
||||
double ProfiledPIDController::GetVelocityError() const {
|
||||
return m_controller.GetVelocityError();
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(units::meter_t measurement) {
|
||||
frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
|
||||
m_setpoint = profile.Calculate(GetPeriod());
|
||||
return m_controller.Calculate(measurement.to<double>(),
|
||||
m_setpoint.position.to<double>());
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(units::meter_t measurement,
|
||||
TrapezoidProfile::State goal) {
|
||||
SetGoal(goal);
|
||||
return Calculate(measurement);
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(units::meter_t measurement,
|
||||
units::meter_t goal) {
|
||||
SetGoal(goal);
|
||||
return Calculate(measurement);
|
||||
}
|
||||
|
||||
double ProfiledPIDController::Calculate(
|
||||
units::meter_t measurement, units::meter_t goal,
|
||||
frc::TrapezoidProfile::Constraints constraints) {
|
||||
SetConstraints(constraints);
|
||||
return Calculate(measurement, goal);
|
||||
}
|
||||
|
||||
void ProfiledPIDController::Reset() { m_controller.Reset(); }
|
||||
|
||||
void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("ProfiledPIDController");
|
||||
builder.AddDoubleProperty("p", [this] { return GetP(); },
|
||||
[this](double value) { SetP(value); });
|
||||
builder.AddDoubleProperty("i", [this] { return GetI(); },
|
||||
[this](double value) { SetI(value); });
|
||||
builder.AddDoubleProperty("d", [this] { return GetD(); },
|
||||
[this](double value) { SetD(value); });
|
||||
builder.AddDoubleProperty(
|
||||
"goal", [this] { return GetGoal().position.to<double>(); },
|
||||
[this](double value) { SetGoal(units::meter_t{value}); });
|
||||
}
|
||||
@@ -1,158 +0,0 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "frc/trajectory/TrapezoidProfile.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
|
||||
State initial)
|
||||
: m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
|
||||
m_constraints(constraints),
|
||||
m_initial(Direct(initial)),
|
||||
m_goal(Direct(goal)) {
|
||||
if (m_initial.velocity > m_constraints.maxVelocity) {
|
||||
m_initial.velocity = m_constraints.maxVelocity;
|
||||
}
|
||||
|
||||
// Deal with a possibly truncated motion profile (with nonzero initial or
|
||||
// final velocity) by calculating the parameters as if the profile began and
|
||||
// ended at zero velocity
|
||||
units::second_t cutoffBegin =
|
||||
m_initial.velocity / m_constraints.maxAcceleration;
|
||||
units::meter_t cutoffDistBegin =
|
||||
cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
|
||||
|
||||
units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
|
||||
units::meter_t cutoffDistEnd =
|
||||
cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
|
||||
|
||||
// Now we can calculate the parameters as if it was a full trapezoid instead
|
||||
// of a truncated one
|
||||
|
||||
units::meter_t fullTrapezoidDist =
|
||||
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
|
||||
units::second_t accelerationTime =
|
||||
m_constraints.maxVelocity / m_constraints.maxAcceleration;
|
||||
|
||||
units::meter_t fullSpeedDist =
|
||||
fullTrapezoidDist -
|
||||
accelerationTime * accelerationTime * m_constraints.maxAcceleration;
|
||||
|
||||
// Handle the case where the profile never reaches full speed
|
||||
if (fullSpeedDist < 0_m) {
|
||||
accelerationTime =
|
||||
units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
|
||||
fullSpeedDist = 0_m;
|
||||
}
|
||||
|
||||
m_endAccel = accelerationTime - cutoffBegin;
|
||||
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
|
||||
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
|
||||
}
|
||||
|
||||
TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
|
||||
State result = m_initial;
|
||||
|
||||
if (t < m_endAccel) {
|
||||
result.velocity += t * m_constraints.maxAcceleration;
|
||||
result.position +=
|
||||
(m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
|
||||
} else if (t < m_endFullSpeed) {
|
||||
result.velocity = m_constraints.maxVelocity;
|
||||
result.position += (m_initial.velocity +
|
||||
m_endAccel * m_constraints.maxAcceleration / 2.0) *
|
||||
m_endAccel +
|
||||
m_constraints.maxVelocity * (t - m_endAccel);
|
||||
} else if (t <= m_endDeccel) {
|
||||
result.velocity =
|
||||
m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
|
||||
units::second_t timeLeft = m_endDeccel - t;
|
||||
result.position =
|
||||
m_goal.position -
|
||||
(m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
|
||||
timeLeft;
|
||||
} else {
|
||||
result = m_goal;
|
||||
}
|
||||
|
||||
return Direct(result);
|
||||
}
|
||||
|
||||
units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
|
||||
units::meter_t position = m_initial.position * m_direction;
|
||||
units::meters_per_second_t velocity = m_initial.velocity * m_direction;
|
||||
|
||||
units::second_t endAccel = m_endAccel * m_direction;
|
||||
units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
|
||||
|
||||
if (target < position) {
|
||||
endAccel *= -1.0;
|
||||
endFullSpeed *= -1.0;
|
||||
velocity *= -1.0;
|
||||
}
|
||||
|
||||
endAccel = units::math::max(endAccel, 0_s);
|
||||
endFullSpeed = units::math::max(endFullSpeed, 0_s);
|
||||
units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
|
||||
endDeccel = units::math::max(endDeccel, 0_s);
|
||||
|
||||
const units::meters_per_second_squared_t acceleration =
|
||||
m_constraints.maxAcceleration;
|
||||
const units::meters_per_second_squared_t decceleration =
|
||||
-m_constraints.maxAcceleration;
|
||||
|
||||
units::meter_t distToTarget = units::math::abs(target - position);
|
||||
|
||||
if (distToTarget < 1e-6_m) {
|
||||
return 0_s;
|
||||
}
|
||||
|
||||
units::meter_t accelDist =
|
||||
velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
|
||||
|
||||
units::meters_per_second_t deccelVelocity;
|
||||
if (endAccel > 0_s) {
|
||||
deccelVelocity = units::math::sqrt(
|
||||
units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
|
||||
} else {
|
||||
deccelVelocity = velocity;
|
||||
}
|
||||
|
||||
units::meter_t deccelDist =
|
||||
deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
|
||||
|
||||
deccelDist = units::math::max(deccelDist, 0_m);
|
||||
|
||||
units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
|
||||
|
||||
if (accelDist > distToTarget) {
|
||||
accelDist = distToTarget;
|
||||
fullSpeedDist = 0_m;
|
||||
deccelDist = 0_m;
|
||||
} else if (accelDist + fullSpeedDist > distToTarget) {
|
||||
fullSpeedDist = distToTarget - accelDist;
|
||||
deccelDist = 0_m;
|
||||
} else {
|
||||
deccelDist = distToTarget - fullSpeedDist - accelDist;
|
||||
}
|
||||
|
||||
units::second_t accelTime =
|
||||
(-velocity + units::math::sqrt(units::math::abs(
|
||||
velocity * velocity + 2 * acceleration * accelDist))) /
|
||||
acceleration;
|
||||
|
||||
units::second_t deccelTime =
|
||||
(-deccelVelocity +
|
||||
units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
|
||||
2 * decceleration * deccelDist))) /
|
||||
decceleration;
|
||||
|
||||
units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
|
||||
|
||||
return accelTime + fullSpeedTime + deccelTime;
|
||||
}
|
||||
Reference in New Issue
Block a user