mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-25 01:41:43 +00:00
[wpimath] Make controllers and some trajectory classes constexpr (#7343)
This commit is contained in:
Tyler Veness
@@ -15,20 +15,6 @@
|
||||
|
||||
using namespace frc;
|
||||
|
||||
units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
|
||||
units::unit_t<Velocity> currentVelocity,
|
||||
units::unit_t<Velocity> nextVelocity,
|
||||
units::second_t dt) const {
|
||||
return Calculate(currentAngle, currentVelocity, nextVelocity);
|
||||
}
|
||||
|
||||
units::volt_t ArmFeedforward::Calculate(
|
||||
units::unit_t<Angle> currentAngle,
|
||||
units::unit_t<Velocity> currentVelocity) const {
|
||||
return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) +
|
||||
kV * currentVelocity;
|
||||
}
|
||||
|
||||
units::volt_t ArmFeedforward::Calculate(
|
||||
units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
|
||||
units::unit_t<Velocity> nextVelocity) const {
|
||||
|
||||
@@ -6,52 +6,8 @@
|
||||
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
BangBangController::BangBangController(double tolerance)
|
||||
: m_tolerance(tolerance) {}
|
||||
|
||||
void BangBangController::SetSetpoint(double setpoint) {
|
||||
m_setpoint = setpoint;
|
||||
}
|
||||
|
||||
double BangBangController::GetSetpoint() const {
|
||||
return m_setpoint;
|
||||
}
|
||||
|
||||
bool BangBangController::AtSetpoint() const {
|
||||
return std::abs(m_setpoint - m_measurement) < m_tolerance;
|
||||
}
|
||||
|
||||
void BangBangController::SetTolerance(double tolerance) {
|
||||
m_tolerance = tolerance;
|
||||
}
|
||||
|
||||
double BangBangController::GetTolerance() const {
|
||||
return m_tolerance;
|
||||
}
|
||||
|
||||
double BangBangController::GetMeasurement() const {
|
||||
return m_measurement;
|
||||
}
|
||||
|
||||
double BangBangController::GetError() const {
|
||||
return m_setpoint - m_measurement;
|
||||
}
|
||||
|
||||
double BangBangController::Calculate(double measurement, double setpoint) {
|
||||
m_measurement = measurement;
|
||||
m_setpoint = setpoint;
|
||||
|
||||
return measurement < setpoint ? 1 : 0;
|
||||
}
|
||||
|
||||
double BangBangController::Calculate(double measurement) {
|
||||
return Calculate(measurement, m_setpoint);
|
||||
}
|
||||
|
||||
void BangBangController::InitSendable(wpi::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("BangBangController");
|
||||
builder.AddDoubleProperty(
|
||||
|
||||
@@ -4,35 +4,10 @@
|
||||
|
||||
#include "frc/controller/DifferentialDriveAccelerationLimiter.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <Eigen/QR>
|
||||
|
||||
using namespace frc;
|
||||
|
||||
DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
|
||||
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
|
||||
units::meters_per_second_squared_t maxLinearAccel,
|
||||
units::radians_per_second_squared_t maxAngularAccel)
|
||||
: DifferentialDriveAccelerationLimiter(system, trackwidth, -maxLinearAccel,
|
||||
maxLinearAccel, maxAngularAccel) {}
|
||||
|
||||
DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter(
|
||||
LinearSystem<2, 2, 2> system, units::meter_t trackwidth,
|
||||
units::meters_per_second_squared_t minLinearAccel,
|
||||
units::meters_per_second_squared_t maxLinearAccel,
|
||||
units::radians_per_second_squared_t maxAngularAccel)
|
||||
: m_system{std::move(system)},
|
||||
m_trackwidth{trackwidth},
|
||||
m_minLinearAccel{minLinearAccel},
|
||||
m_maxLinearAccel{maxLinearAccel},
|
||||
m_maxAngularAccel{maxAngularAccel} {
|
||||
if (minLinearAccel > maxLinearAccel) {
|
||||
throw std::invalid_argument(
|
||||
"maxLinearAccel must be greater than minLinearAccel");
|
||||
}
|
||||
}
|
||||
|
||||
DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
|
||||
units::meters_per_second_t leftVelocity,
|
||||
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
|
||||
|
||||
@@ -7,31 +7,9 @@
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "frc/controller/LinearPlantInversionFeedforward.h"
|
||||
#include "frc/system/plant/LinearSystemId.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
DifferentialDriveFeedforward::DifferentialDriveFeedforward(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_rad_per_s) kVAngular,
|
||||
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
|
||||
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
|
||||
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
|
||||
// 1_rad_per_s_sq))
|
||||
: DifferentialDriveFeedforward{kVLinear, kALinear,
|
||||
kVAngular * 2.0 / trackwidth * 1_rad,
|
||||
kAAngular * 2.0 / trackwidth * 1_rad} {}
|
||||
|
||||
DifferentialDriveFeedforward::DifferentialDriveFeedforward(
|
||||
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
|
||||
decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular)
|
||||
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
|
||||
kVLinear, kALinear, kVAngular, kAAngular)},
|
||||
m_kVLinear{kVLinear},
|
||||
m_kALinear{kALinear},
|
||||
m_kVAngular{kVAngular},
|
||||
m_kAAngular{kAAngular} {}
|
||||
|
||||
DifferentialDriveWheelVoltages DifferentialDriveFeedforward::Calculate(
|
||||
units::meters_per_second_t currentLeftVelocity,
|
||||
units::meters_per_second_t nextLeftVelocity,
|
||||
|
||||
@@ -1,107 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/controller/HolonomicDriveController.h"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "units/angular_velocity.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
HolonomicDriveController::HolonomicDriveController(
|
||||
PIDController xController, PIDController yController,
|
||||
ProfiledPIDController<units::radian> thetaController)
|
||||
: m_xController(std::move(xController)),
|
||||
m_yController(std::move(yController)),
|
||||
m_thetaController(std::move(thetaController)) {
|
||||
m_thetaController.EnableContinuousInput(0_deg, 360.0_deg);
|
||||
}
|
||||
|
||||
bool HolonomicDriveController::AtReference() const {
|
||||
const auto& eTranslate = m_poseError.Translation();
|
||||
const auto& eRotate = m_rotationError;
|
||||
const auto& tolTranslate = m_poseTolerance.Translation();
|
||||
const auto& tolRotate = m_poseTolerance.Rotation();
|
||||
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
|
||||
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
|
||||
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
|
||||
}
|
||||
|
||||
void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
|
||||
m_poseTolerance = tolerance;
|
||||
}
|
||||
|
||||
ChassisSpeeds HolonomicDriveController::Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& trajectoryPose,
|
||||
units::meters_per_second_t desiredLinearVelocity,
|
||||
const Rotation2d& desiredHeading) {
|
||||
// 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.Rotation().Radians());
|
||||
m_firstRun = false;
|
||||
}
|
||||
|
||||
// Calculate feedforward velocities (field-relative)
|
||||
auto xFF = desiredLinearVelocity * trajectoryPose.Rotation().Cos();
|
||||
auto yFF = desiredLinearVelocity * trajectoryPose.Rotation().Sin();
|
||||
auto thetaFF = units::radians_per_second_t{m_thetaController.Calculate(
|
||||
currentPose.Rotation().Radians(), desiredHeading.Radians())};
|
||||
|
||||
m_poseError = trajectoryPose.RelativeTo(currentPose);
|
||||
m_rotationError = desiredHeading - currentPose.Rotation();
|
||||
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
|
||||
currentPose.Rotation());
|
||||
}
|
||||
|
||||
// Calculate feedback velocities (based on position error).
|
||||
auto xFeedback = units::meters_per_second_t{m_xController.Calculate(
|
||||
currentPose.X().value(), trajectoryPose.X().value())};
|
||||
auto yFeedback = units::meters_per_second_t{m_yController.Calculate(
|
||||
currentPose.Y().value(), trajectoryPose.Y().value())};
|
||||
|
||||
// Return next output.
|
||||
return ChassisSpeeds::FromFieldRelativeSpeeds(
|
||||
xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
|
||||
}
|
||||
|
||||
ChassisSpeeds HolonomicDriveController::Calculate(
|
||||
const Pose2d& currentPose, const Trajectory::State& desiredState,
|
||||
const Rotation2d& desiredHeading) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredHeading);
|
||||
}
|
||||
|
||||
void HolonomicDriveController::SetEnabled(bool enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
|
||||
PIDController& HolonomicDriveController::getXController() {
|
||||
return m_xController;
|
||||
}
|
||||
|
||||
PIDController& HolonomicDriveController::getYController() {
|
||||
return m_yController;
|
||||
}
|
||||
|
||||
ProfiledPIDController<units::radian>&
|
||||
HolonomicDriveController::getThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
|
||||
PIDController& HolonomicDriveController::GetXController() {
|
||||
return m_xController;
|
||||
}
|
||||
|
||||
PIDController& HolonomicDriveController::GetYController() {
|
||||
return m_yController;
|
||||
}
|
||||
|
||||
ProfiledPIDController<units::radian>&
|
||||
HolonomicDriveController::GetThetaController() {
|
||||
return m_thetaController;
|
||||
}
|
||||
@@ -4,232 +4,10 @@
|
||||
|
||||
#include "frc/controller/PIDController.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
#include <wpi/sendable/SendableBuilder.h>
|
||||
#include <wpi/sendable/SendableRegistry.h>
|
||||
|
||||
#include "frc/MathUtil.h"
|
||||
#include "wpimath/MathShared.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
PIDController::PIDController(double Kp, double Ki, double Kd,
|
||||
units::second_t period)
|
||||
: m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
|
||||
bool invalidGains = false;
|
||||
if (Kp < 0.0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Kp must be a non-negative number, got {}!", Kp);
|
||||
invalidGains = true;
|
||||
}
|
||||
if (Ki < 0.0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Ki must be a non-negative number, got {}!", Ki);
|
||||
invalidGains = true;
|
||||
}
|
||||
if (Kd < 0.0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Kd must be a non-negative number, got {}!", Kd);
|
||||
invalidGains = true;
|
||||
}
|
||||
if (invalidGains) {
|
||||
m_Kp = 0.0;
|
||||
m_Ki = 0.0;
|
||||
m_Kd = 0.0;
|
||||
wpi::math::MathSharedStore::ReportWarning("PID gains defaulted to 0.");
|
||||
}
|
||||
|
||||
if (period <= 0_s) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"Controller period must be a positive number, got {}!", period.value());
|
||||
m_period = 20_ms;
|
||||
wpi::math::MathSharedStore::ReportWarning(
|
||||
"Controller period defaulted to 20ms.");
|
||||
}
|
||||
static int instances = 0;
|
||||
instances++;
|
||||
|
||||
wpi::math::MathSharedStore::ReportUsage(
|
||||
wpi::math::MathUsageId::kController_PIDController2, instances);
|
||||
wpi::SendableRegistry::Add(this, "PIDController", instances);
|
||||
}
|
||||
|
||||
void PIDController::SetPID(double Kp, double Ki, double Kd) {
|
||||
m_Kp = Kp;
|
||||
m_Ki = Ki;
|
||||
m_Kd = Kd;
|
||||
}
|
||||
|
||||
void PIDController::SetP(double Kp) {
|
||||
m_Kp = Kp;
|
||||
}
|
||||
|
||||
void PIDController::SetI(double Ki) {
|
||||
m_Ki = Ki;
|
||||
}
|
||||
|
||||
void PIDController::SetD(double Kd) {
|
||||
m_Kd = Kd;
|
||||
}
|
||||
|
||||
void PIDController::SetIZone(double iZone) {
|
||||
if (iZone < 0) {
|
||||
wpi::math::MathSharedStore::ReportError(
|
||||
"IZone must be a non-negative number, got {}!", iZone);
|
||||
}
|
||||
m_iZone = iZone;
|
||||
}
|
||||
|
||||
double PIDController::GetP() const {
|
||||
return m_Kp;
|
||||
}
|
||||
|
||||
double PIDController::GetI() const {
|
||||
return m_Ki;
|
||||
}
|
||||
|
||||
double PIDController::GetD() const {
|
||||
return m_Kd;
|
||||
}
|
||||
|
||||
double PIDController::GetIZone() const {
|
||||
return m_iZone;
|
||||
}
|
||||
|
||||
units::second_t PIDController::GetPeriod() const {
|
||||
return m_period;
|
||||
}
|
||||
|
||||
double PIDController::GetPositionTolerance() const {
|
||||
return m_errorTolerance;
|
||||
}
|
||||
|
||||
double PIDController::GetVelocityTolerance() const {
|
||||
return m_errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
double PIDController::GetAccumulatedError() const {
|
||||
return m_totalError;
|
||||
}
|
||||
|
||||
void PIDController::SetSetpoint(double setpoint) {
|
||||
m_setpoint = setpoint;
|
||||
m_haveSetpoint = true;
|
||||
|
||||
if (m_continuous) {
|
||||
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
|
||||
} else {
|
||||
m_error = m_setpoint - m_measurement;
|
||||
}
|
||||
|
||||
m_errorDerivative = (m_error - m_prevError) / m_period.value();
|
||||
}
|
||||
|
||||
double PIDController::GetSetpoint() const {
|
||||
return m_setpoint;
|
||||
}
|
||||
|
||||
bool PIDController::AtSetpoint() const {
|
||||
return m_haveMeasurement && m_haveSetpoint &&
|
||||
std::abs(m_error) < m_errorTolerance &&
|
||||
std::abs(m_errorDerivative) < m_errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
void PIDController::EnableContinuousInput(double minimumInput,
|
||||
double maximumInput) {
|
||||
m_continuous = true;
|
||||
m_minimumInput = minimumInput;
|
||||
m_maximumInput = maximumInput;
|
||||
}
|
||||
|
||||
void PIDController::DisableContinuousInput() {
|
||||
m_continuous = false;
|
||||
}
|
||||
|
||||
bool PIDController::IsContinuousInputEnabled() const {
|
||||
return m_continuous;
|
||||
}
|
||||
|
||||
void PIDController::SetIntegratorRange(double minimumIntegral,
|
||||
double maximumIntegral) {
|
||||
m_minimumIntegral = minimumIntegral;
|
||||
m_maximumIntegral = maximumIntegral;
|
||||
}
|
||||
|
||||
void PIDController::SetTolerance(double errorTolerance,
|
||||
double errorDerivativeTolerance) {
|
||||
m_errorTolerance = errorTolerance;
|
||||
m_errorDerivativeTolerance = errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
double PIDController::GetErrorTolerance() const {
|
||||
return m_errorTolerance;
|
||||
}
|
||||
|
||||
double PIDController::GetErrorDerivativeTolerance() const {
|
||||
return m_errorDerivativeTolerance;
|
||||
}
|
||||
|
||||
double PIDController::GetError() const {
|
||||
return m_error;
|
||||
}
|
||||
|
||||
double PIDController::GetErrorDerivative() const {
|
||||
return m_errorDerivative;
|
||||
}
|
||||
|
||||
double PIDController::GetPositionError() const {
|
||||
return m_error;
|
||||
}
|
||||
|
||||
double PIDController::GetVelocityError() const {
|
||||
return m_errorDerivative;
|
||||
}
|
||||
|
||||
double PIDController::Calculate(double measurement) {
|
||||
m_measurement = measurement;
|
||||
m_prevError = m_error;
|
||||
m_haveMeasurement = true;
|
||||
|
||||
if (m_continuous) {
|
||||
double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
|
||||
m_error = InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
|
||||
} else {
|
||||
m_error = m_setpoint - m_measurement;
|
||||
}
|
||||
|
||||
m_errorDerivative = (m_error - m_prevError) / m_period.value();
|
||||
|
||||
// If the absolute value of the position error is outside of IZone, reset the
|
||||
// total error
|
||||
if (std::abs(m_error) > m_iZone) {
|
||||
m_totalError = 0;
|
||||
} else if (m_Ki != 0) {
|
||||
m_totalError =
|
||||
std::clamp(m_totalError + m_error * m_period.value(),
|
||||
m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
|
||||
}
|
||||
|
||||
return m_Kp * m_error + m_Ki * m_totalError + m_Kd * m_errorDerivative;
|
||||
}
|
||||
|
||||
double PIDController::Calculate(double measurement, double setpoint) {
|
||||
m_setpoint = setpoint;
|
||||
m_haveSetpoint = true;
|
||||
return Calculate(measurement);
|
||||
}
|
||||
|
||||
void PIDController::Reset() {
|
||||
m_error = 0;
|
||||
m_prevError = 0;
|
||||
m_totalError = 0;
|
||||
m_errorDerivative = 0;
|
||||
m_haveMeasurement = false;
|
||||
}
|
||||
|
||||
void PIDController::InitSendable(wpi::SendableBuilder& builder) {
|
||||
builder.SetSmartDashboardType("PIDController");
|
||||
builder.AddDoubleProperty(
|
||||
|
||||
@@ -1,89 +0,0 @@
|
||||
// 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.
|
||||
|
||||
#include "frc/controller/RamseteController.h"
|
||||
|
||||
#include <wpi/deprecated.h>
|
||||
|
||||
#include "units/angle.h"
|
||||
#include "units/math.h"
|
||||
|
||||
using namespace frc;
|
||||
|
||||
/**
|
||||
* Returns sin(x) / x.
|
||||
*
|
||||
* @param x Value of which to take sinc(x).
|
||||
*/
|
||||
static decltype(1 / 1_rad) Sinc(units::radian_t x) {
|
||||
if (units::math::abs(x) < 1e-9_rad) {
|
||||
return decltype(1 / 1_rad){1.0 - 1.0 / 6.0 * x.value() * x.value()};
|
||||
} else {
|
||||
return units::math::sin(x) / x;
|
||||
}
|
||||
}
|
||||
|
||||
RamseteController::RamseteController(units::unit_t<b_unit> b,
|
||||
units::unit_t<zeta_unit> zeta)
|
||||
: m_b{b}, m_zeta{zeta} {}
|
||||
|
||||
WPI_IGNORE_DEPRECATED
|
||||
|
||||
RamseteController::RamseteController()
|
||||
: RamseteController{units::unit_t<b_unit>{2.0},
|
||||
units::unit_t<zeta_unit>{0.7}} {}
|
||||
|
||||
WPI_UNIGNORE_DEPRECATED
|
||||
|
||||
bool RamseteController::AtReference() const {
|
||||
const auto& eTranslate = m_poseError.Translation();
|
||||
const auto& eRotate = m_poseError.Rotation();
|
||||
const auto& tolTranslate = m_poseTolerance.Translation();
|
||||
const auto& tolRotate = m_poseTolerance.Rotation();
|
||||
return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
|
||||
units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
|
||||
units::math::abs(eRotate.Radians()) < tolRotate.Radians();
|
||||
}
|
||||
|
||||
void RamseteController::SetTolerance(const Pose2d& poseTolerance) {
|
||||
m_poseTolerance = poseTolerance;
|
||||
}
|
||||
|
||||
ChassisSpeeds RamseteController::Calculate(
|
||||
const Pose2d& currentPose, const Pose2d& poseRef,
|
||||
units::meters_per_second_t linearVelocityRef,
|
||||
units::radians_per_second_t angularVelocityRef) {
|
||||
if (!m_enabled) {
|
||||
return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
|
||||
}
|
||||
|
||||
m_poseError = poseRef.RelativeTo(currentPose);
|
||||
|
||||
// Aliases for equation readability
|
||||
const auto& eX = m_poseError.X();
|
||||
const auto& eY = m_poseError.Y();
|
||||
const auto& eTheta = m_poseError.Rotation().Radians();
|
||||
const auto& vRef = linearVelocityRef;
|
||||
const auto& omegaRef = angularVelocityRef;
|
||||
|
||||
// k = 2ζ√(ω_ref² + b v_ref²)
|
||||
auto k = 2.0 * m_zeta *
|
||||
units::math::sqrt(units::math::pow<2>(omegaRef) +
|
||||
m_b * units::math::pow<2>(vRef));
|
||||
|
||||
// v_cmd = v_ref cos(e_θ) + k e_x
|
||||
// ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
|
||||
return ChassisSpeeds{vRef * m_poseError.Rotation().Cos() + k * eX, 0_mps,
|
||||
omegaRef + k * eTheta + m_b * vRef * Sinc(eTheta) * eY};
|
||||
}
|
||||
|
||||
ChassisSpeeds RamseteController::Calculate(
|
||||
const Pose2d& currentPose, const Trajectory::State& desiredState) {
|
||||
return Calculate(currentPose, desiredState.pose, desiredState.velocity,
|
||||
desiredState.velocity * desiredState.curvature);
|
||||
}
|
||||
|
||||
void RamseteController::SetEnabled(bool enabled) {
|
||||
m_enabled = enabled;
|
||||
}
|
||||
Reference in New Issue
Block a user