mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-23 01:21:42 +00:00
[wpimath] Move controller from wpilibj to wpimath (#3439)
This commit is contained in:
Noam Zaks
@@ -0,0 +1,110 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc/controller/ProfiledPIDController.h"
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/geometry/Rotation2d.h"
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/angle.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
class HolonomicDriveController {
|
||||
public:
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
HolonomicDriveController(
|
||||
frc2::PIDController xController, frc2::PIDController yController,
|
||||
ProfiledPIDController<units::radian> thetaController);
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
bool AtReference() const;
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with
|
||||
* AtReference().
|
||||
*
|
||||
* @param poseTolerance Pose error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(const Pose2d& tolerance);
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* The reference pose, linear velocity, and angular velocity should come from
|
||||
* a drivetrain trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param poseRef The desired pose.
|
||||
* @param linearVelocityRef The desired linear velocity.
|
||||
* @param angleRef The desired ending angle.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
const Rotation2d& angleRef);
|
||||
|
||||
/**
|
||||
* Returns the next output of the holonomic drive controller.
|
||||
*
|
||||
* The reference pose, linear velocity, and angular velocity should come from
|
||||
* a drivetrain trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param desiredState The desired pose, linear velocity, and angular velocity
|
||||
* from a trajectory.
|
||||
* @param angleRef The desired ending angle.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState,
|
||||
const Rotation2d& angleRef);
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes. When
|
||||
* Calculate() is called on a disabled controller, only feedforward values
|
||||
* are returned.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
void SetEnabled(bool enabled);
|
||||
|
||||
private:
|
||||
Pose2d m_poseError;
|
||||
Rotation2d m_rotationError;
|
||||
Pose2d m_poseTolerance;
|
||||
bool m_enabled = true;
|
||||
|
||||
frc2::PIDController m_xController;
|
||||
frc2::PIDController m_yController;
|
||||
ProfiledPIDController<units::radian> m_thetaController;
|
||||
|
||||
bool m_firstRun = true;
|
||||
};
|
||||
} // namespace frc
|
||||
247
wpimath/src/main/native/include/frc/controller/PIDController.h
Normal file
247
wpimath/src/main/native/include/frc/controller/PIDController.h
Normal file
@@ -0,0 +1,247 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc2 {
|
||||
|
||||
/**
|
||||
* Implements a PID control loop.
|
||||
*/
|
||||
class PIDController : public wpi::Sendable,
|
||||
public wpi::SendableHelper<PIDController> {
|
||||
public:
|
||||
/**
|
||||
* 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. The
|
||||
* default is 20 milliseconds. Must be non-zero and positive.
|
||||
*/
|
||||
PIDController(double Kp, double Ki, double Kd,
|
||||
units::second_t period = 20_ms);
|
||||
|
||||
~PIDController() override = default;
|
||||
|
||||
PIDController(const PIDController&) = default;
|
||||
PIDController& operator=(const PIDController&) = default;
|
||||
PIDController(PIDController&&) = default;
|
||||
PIDController& operator=(PIDController&&) = default;
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
*
|
||||
* Sets the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param Kp Proportional coefficient
|
||||
* @param Ki Integral coefficient
|
||||
* @param Kd Differential coefficient
|
||||
*/
|
||||
void SetPID(double Kp, double Ki, double Kd);
|
||||
|
||||
/**
|
||||
* Sets the proportional coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kp proportional coefficient
|
||||
*/
|
||||
void SetP(double Kp);
|
||||
|
||||
/**
|
||||
* Sets the integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Ki integral coefficient
|
||||
*/
|
||||
void SetI(double Ki);
|
||||
|
||||
/**
|
||||
* Sets the differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kd differential coefficient
|
||||
*/
|
||||
void SetD(double Kd);
|
||||
|
||||
/**
|
||||
* Gets the proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
double GetP() const;
|
||||
|
||||
/**
|
||||
* Gets the integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
double GetI() const;
|
||||
|
||||
/**
|
||||
* Gets the differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
double GetD() const;
|
||||
|
||||
/**
|
||||
* Gets the period of this controller.
|
||||
*
|
||||
* @return The period of the controller.
|
||||
*/
|
||||
units::second_t GetPeriod() const;
|
||||
|
||||
/**
|
||||
* Sets the setpoint for the PIDController.
|
||||
*
|
||||
* @param setpoint The desired setpoint.
|
||||
*/
|
||||
void SetSetpoint(double setpoint);
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the PIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
double GetSetpoint() const;
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the setpoint.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtSetpoint() const;
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
void EnableContinuousInput(double minimumInput, double maximumInput);
|
||||
|
||||
/**
|
||||
* Disables continuous input.
|
||||
*/
|
||||
void DisableContinuousInput();
|
||||
|
||||
/**
|
||||
* Returns true if continuous input is enabled.
|
||||
*/
|
||||
bool IsContinuousInputEnabled() const;
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values for the integrator.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
|
||||
|
||||
/**
|
||||
* Sets the error which is considered tolerable for use with AtSetpoint().
|
||||
*
|
||||
* @param positionTolerance Position error which is tolerable.
|
||||
* @param velociytTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(
|
||||
double positionTolerance,
|
||||
double velocityTolerance = std::numeric_limits<double>::infinity());
|
||||
|
||||
/**
|
||||
* Returns the difference between the setpoint and the measurement.
|
||||
*/
|
||||
double GetPositionError() const;
|
||||
|
||||
/**
|
||||
* Returns the velocity error.
|
||||
*/
|
||||
double GetVelocityError() const;
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
*/
|
||||
double Calculate(double measurement);
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
double Calculate(double measurement, double setpoint);
|
||||
|
||||
/**
|
||||
* Reset the previous error, the integral term, and disable the controller.
|
||||
*/
|
||||
void Reset();
|
||||
|
||||
void InitSendable(wpi::SendableBuilder& builder) override;
|
||||
|
||||
private:
|
||||
// Factor for "proportional" control
|
||||
double m_Kp;
|
||||
|
||||
// Factor for "integral" control
|
||||
double m_Ki;
|
||||
|
||||
// Factor for "derivative" control
|
||||
double m_Kd;
|
||||
|
||||
// The period (in seconds) of the control loop running this controller
|
||||
units::second_t m_period;
|
||||
|
||||
double m_maximumIntegral = 1.0;
|
||||
|
||||
double m_minimumIntegral = -1.0;
|
||||
|
||||
double m_maximumInput = 0;
|
||||
|
||||
double m_minimumInput = 0;
|
||||
|
||||
// Do the endpoints wrap around? eg. Absolute encoder
|
||||
bool m_continuous = false;
|
||||
|
||||
// The error at the time of the most recent call to Calculate()
|
||||
double m_positionError = 0;
|
||||
double m_velocityError = 0;
|
||||
|
||||
// The error at the time of the second-most-recent call to Calculate() (used
|
||||
// to compute velocity)
|
||||
double m_prevError = 0;
|
||||
|
||||
// The sum of the errors for use in the integral calc
|
||||
double m_totalError = 0;
|
||||
|
||||
// The error that is considered at setpoint.
|
||||
double m_positionTolerance = 0.05;
|
||||
double m_velocityTolerance = std::numeric_limits<double>::infinity();
|
||||
|
||||
double m_setpoint = 0;
|
||||
double m_measurement = 0;
|
||||
};
|
||||
|
||||
} // namespace frc2
|
||||
|
||||
namespace frc {
|
||||
|
||||
using frc2::PIDController;
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,365 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
|
||||
#include <wpi/sendable/Sendable.h>
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
#include <wpi/sendable/SendableHelper.h>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "frc/controller/PIDController.h"
|
||||
#include "frc/trajectory/TrapezoidProfile.h"
|
||||
#include "units/time.h"
|
||||
|
||||
namespace frc {
|
||||
namespace detail {
|
||||
void ReportProfiledPIDController();
|
||||
} // namespace detail
|
||||
|
||||
/**
|
||||
* Implements a PID control loop whose setpoint is constrained by a trapezoid
|
||||
* profile.
|
||||
*/
|
||||
template <class Distance>
|
||||
class ProfiledPIDController
|
||||
: public wpi::Sendable,
|
||||
public wpi::SendableHelper<ProfiledPIDController<Distance>> {
|
||||
public:
|
||||
using Distance_t = units::unit_t<Distance>;
|
||||
using Velocity =
|
||||
units::compound_unit<Distance, units::inverse<units::seconds>>;
|
||||
using Velocity_t = units::unit_t<Velocity>;
|
||||
using Acceleration =
|
||||
units::compound_unit<Velocity, units::inverse<units::seconds>>;
|
||||
using Acceleration_t = units::unit_t<Acceleration>;
|
||||
using State = typename TrapezoidProfile<Distance>::State;
|
||||
using Constraints = typename TrapezoidProfile<Distance>::Constraints;
|
||||
|
||||
/**
|
||||
* Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
|
||||
* Kd. Users should call reset() when they first start running the controller
|
||||
* to avoid unwanted behavior.
|
||||
*
|
||||
* @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 20 milliseconds.
|
||||
*/
|
||||
ProfiledPIDController(double Kp, double Ki, double Kd,
|
||||
Constraints constraints, units::second_t period = 20_ms)
|
||||
: m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
|
||||
detail::ReportProfiledPIDController();
|
||||
}
|
||||
|
||||
~ProfiledPIDController() override = default;
|
||||
|
||||
ProfiledPIDController(const ProfiledPIDController&) = default;
|
||||
ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
|
||||
ProfiledPIDController(ProfiledPIDController&&) = default;
|
||||
ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
|
||||
|
||||
/**
|
||||
* Sets the PID Controller gain parameters.
|
||||
*
|
||||
* Sets the proportional, integral, and differential coefficients.
|
||||
*
|
||||
* @param Kp Proportional coefficient
|
||||
* @param Ki Integral coefficient
|
||||
* @param Kd Differential coefficient
|
||||
*/
|
||||
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
|
||||
*/
|
||||
void SetP(double Kp) { m_controller.SetP(Kp); }
|
||||
|
||||
/**
|
||||
* Sets the integral coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Ki integral coefficient
|
||||
*/
|
||||
void SetI(double Ki) { m_controller.SetI(Ki); }
|
||||
|
||||
/**
|
||||
* Sets the differential coefficient of the PID controller gain.
|
||||
*
|
||||
* @param Kd differential coefficient
|
||||
*/
|
||||
void SetD(double Kd) { m_controller.SetD(Kd); }
|
||||
|
||||
/**
|
||||
* Gets the proportional coefficient.
|
||||
*
|
||||
* @return proportional coefficient
|
||||
*/
|
||||
double GetP() const { return m_controller.GetP(); }
|
||||
|
||||
/**
|
||||
* Gets the integral coefficient.
|
||||
*
|
||||
* @return integral coefficient
|
||||
*/
|
||||
double GetI() const { return m_controller.GetI(); }
|
||||
|
||||
/**
|
||||
* Gets the differential coefficient.
|
||||
*
|
||||
* @return differential coefficient
|
||||
*/
|
||||
double GetD() const { return m_controller.GetD(); }
|
||||
|
||||
/**
|
||||
* Gets the period of this controller.
|
||||
*
|
||||
* @return The period of the controller.
|
||||
*/
|
||||
units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
|
||||
|
||||
/**
|
||||
* Sets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @param goal The desired unprofiled setpoint.
|
||||
*/
|
||||
void SetGoal(State goal) { m_goal = goal; }
|
||||
|
||||
/**
|
||||
* Sets the goal for the ProfiledPIDController.
|
||||
*
|
||||
* @param goal The desired unprofiled setpoint.
|
||||
*/
|
||||
void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
|
||||
|
||||
/**
|
||||
* Gets the goal for the ProfiledPIDController.
|
||||
*/
|
||||
State GetGoal() const { return m_goal; }
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
|
||||
|
||||
/**
|
||||
* Set velocity and acceleration constraints for goal.
|
||||
*
|
||||
* @param constraints Velocity and acceleration constraints for goal.
|
||||
*/
|
||||
void SetConstraints(Constraints constraints) { m_constraints = constraints; }
|
||||
|
||||
/**
|
||||
* Returns the current setpoint of the ProfiledPIDController.
|
||||
*
|
||||
* @return The current setpoint.
|
||||
*/
|
||||
State GetSetpoint() const { return m_setpoint; }
|
||||
|
||||
/**
|
||||
* Returns true if the error is within the tolerance of the error.
|
||||
*
|
||||
* Currently this just reports on target as the actual value passes through
|
||||
* the setpoint. Ideally it should be based on being within the tolerance for
|
||||
* some period of time.
|
||||
*
|
||||
* This will return false until at least one input value has been computed.
|
||||
*/
|
||||
bool AtSetpoint() const { return m_controller.AtSetpoint(); }
|
||||
|
||||
/**
|
||||
* Enables continuous input.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
|
||||
m_controller.EnableContinuousInput(minimumInput.template to<double>(),
|
||||
maximumInput.template to<double>());
|
||||
m_minimumInput = minimumInput;
|
||||
m_maximumInput = maximumInput;
|
||||
}
|
||||
|
||||
/**
|
||||
* Disables continuous input.
|
||||
*/
|
||||
void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
|
||||
|
||||
/**
|
||||
* Sets the minimum and maximum values for the integrator.
|
||||
*
|
||||
* 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.
|
||||
*/
|
||||
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.
|
||||
* @param velocityTolerance Velocity error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(
|
||||
Distance_t positionTolerance,
|
||||
Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
|
||||
m_controller.SetTolerance(positionTolerance.template to<double>(),
|
||||
velocityTolerance.template to<double>());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the difference between the setpoint and the measurement.
|
||||
*
|
||||
* @return The error.
|
||||
*/
|
||||
Distance_t GetPositionError() const {
|
||||
return Distance_t(m_controller.GetPositionError());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the change in error per second.
|
||||
*/
|
||||
Velocity_t GetVelocityError() const {
|
||||
return Velocity_t(m_controller.GetVelocityError());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the next output of the PID controller.
|
||||
*
|
||||
* @param measurement The current measurement of the process variable.
|
||||
*/
|
||||
double Calculate(Distance_t measurement) {
|
||||
if (m_controller.IsContinuousInputEnabled()) {
|
||||
// Get error which is smallest distance between goal and measurement
|
||||
auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
auto goalMinDistance = frc::InputModulus<Distance_t>(
|
||||
m_goal.position - measurement, -errorBound, errorBound);
|
||||
auto setpointMinDistance = frc::InputModulus<Distance_t>(
|
||||
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;
|
||||
}
|
||||
|
||||
frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
|
||||
m_setpoint = profile.Calculate(GetPeriod());
|
||||
return m_controller.Calculate(measurement.template to<double>(),
|
||||
m_setpoint.position.template to<double>());
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
double Calculate(Distance_t measurement, State 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.
|
||||
*/
|
||||
double Calculate(Distance_t measurement, Distance_t 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.
|
||||
*/
|
||||
double Calculate(
|
||||
Distance_t measurement, Distance_t goal,
|
||||
typename frc::TrapezoidProfile<Distance>::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.
|
||||
*/
|
||||
void Reset(const 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.
|
||||
*/
|
||||
void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
|
||||
Reset(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.
|
||||
*/
|
||||
void Reset(Distance_t measuredPosition) {
|
||||
Reset(measuredPosition, Velocity_t(0));
|
||||
}
|
||||
|
||||
void InitSendable(wpi::SendableBuilder& builder) override {
|
||||
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.template to<double>(); },
|
||||
[this](double value) { SetGoal(Distance_t{value}); });
|
||||
}
|
||||
|
||||
private:
|
||||
frc2::PIDController m_controller;
|
||||
Distance_t m_minimumInput{0};
|
||||
Distance_t m_maximumInput{0};
|
||||
typename frc::TrapezoidProfile<Distance>::State m_goal;
|
||||
typename frc::TrapezoidProfile<Distance>::State m_setpoint;
|
||||
typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
@@ -0,0 +1,118 @@
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "frc/geometry/Pose2d.h"
|
||||
#include "frc/kinematics/ChassisSpeeds.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "units/angular_velocity.h"
|
||||
#include "units/velocity.h"
|
||||
|
||||
namespace frc {
|
||||
|
||||
/**
|
||||
* Ramsete is a nonlinear time-varying feedback controller for unicycle models
|
||||
* that drives the model to a desired pose along a two-dimensional trajectory.
|
||||
* Why would we need a nonlinear control law in addition to the linear ones we
|
||||
* have used so far like PID? If we use the original approach with PID
|
||||
* controllers for left and right position and velocity states, the controllers
|
||||
* only deal with the local pose. If the robot deviates from the path, there is
|
||||
* no way for the controllers to correct and the robot may not reach the desired
|
||||
* global pose. This is due to multiple endpoints existing for the robot which
|
||||
* have the same encoder path arc lengths.
|
||||
*
|
||||
* Instead of using wheel path arc lengths (which are in the robot's local
|
||||
* coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
|
||||
* global pose. The controller uses this extra information to guide a linear
|
||||
* reference tracker like the PID controllers back in by adjusting the
|
||||
* references of the PID controllers.
|
||||
*
|
||||
* The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
|
||||
* describes a nonlinear controller for a wheeled vehicle with unicycle-like
|
||||
* kinematics; a global pose consisting of x, y, and theta; and a desired pose
|
||||
* consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
|
||||
* acronym for the title of the book it came from in Italian ("Robotica
|
||||
* Articolata e Mobile per i SErvizi e le TEcnologie").
|
||||
*
|
||||
* See <https://file.tavsys.net/control/controls-engineering-in-frc.pdf> section
|
||||
* on Ramsete unicycle controller for a derivation and analysis.
|
||||
*/
|
||||
class RamseteController {
|
||||
public:
|
||||
/**
|
||||
* Construct a Ramsete unicycle controller.
|
||||
*
|
||||
* @param b Tuning parameter (b > 0) for which larger values make
|
||||
* convergence more aggressive like a proportional term.
|
||||
* @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide
|
||||
* more damping in response.
|
||||
*/
|
||||
RamseteController(double b, double zeta);
|
||||
|
||||
/**
|
||||
* Construct a Ramsete unicycle controller. The default arguments for
|
||||
* b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
|
||||
* results.
|
||||
*/
|
||||
RamseteController() : RamseteController(2.0, 0.7) {}
|
||||
|
||||
/**
|
||||
* Returns true if the pose error is within tolerance of the reference.
|
||||
*/
|
||||
bool AtReference() const;
|
||||
|
||||
/**
|
||||
* Sets the pose error which is considered tolerable for use with
|
||||
* AtReference().
|
||||
*
|
||||
* @param poseTolerance Pose error which is tolerable.
|
||||
*/
|
||||
void SetTolerance(const Pose2d& poseTolerance);
|
||||
|
||||
/**
|
||||
* Returns the next output of the Ramsete controller.
|
||||
*
|
||||
* The reference pose, linear velocity, and angular velocity should come from
|
||||
* a drivetrain trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param poseRef The desired pose.
|
||||
* @param linearVelocityRef The desired linear velocity.
|
||||
* @param angularVelocityRef The desired angular velocity.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef);
|
||||
|
||||
/**
|
||||
* Returns the next output of the Ramsete controller.
|
||||
*
|
||||
* The reference pose, linear velocity, and angular velocity should come from
|
||||
* a drivetrain trajectory.
|
||||
*
|
||||
* @param currentPose The current pose.
|
||||
* @param desiredState The desired pose, linear velocity, and angular velocity
|
||||
* from a trajectory.
|
||||
*/
|
||||
ChassisSpeeds Calculate(const Pose2d& currentPose,
|
||||
const Trajectory::State& desiredState);
|
||||
|
||||
/**
|
||||
* Enables and disables the controller for troubleshooting purposes.
|
||||
*
|
||||
* @param enabled If the controller is enabled or not.
|
||||
*/
|
||||
void SetEnabled(bool enabled);
|
||||
|
||||
private:
|
||||
double m_b;
|
||||
double m_zeta;
|
||||
|
||||
Pose2d m_poseError;
|
||||
Pose2d m_poseTolerance;
|
||||
bool m_enabled = true;
|
||||
};
|
||||
|
||||
} // namespace frc
|
||||
Reference in New Issue
Block a user