Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add minLinearAccel parameter to DifferentialDriveAccelerationLimiter (#4422)

Browse Source
This commit is contained in:
Ryan Blue
2022-10-10 11:57:37 -04:00
committed by GitHub
parent 2a13dba8ac
commit 27b173374e
5 changed files with 246 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp
View File

@@ -14,10 +14,24 @@ 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} {}
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,
@@ -48,8 +62,8 @@ DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate(
// Constrain the linear and angular accelerations
if (accels(0) > m_maxLinearAccel.value()) {
accels(0) = m_maxLinearAccel.value();
} else if (accels(0) < -m_maxLinearAccel.value()) {
accels(0) = -m_maxLinearAccel.value();
} else if (accels(0) < m_minLinearAccel.value()) {
accels(0) = m_minLinearAccel.value();
}
if (accels(1) > m_maxAngularAccel.value()) {
accels(1) = m_maxAngularAccel.value();

19
wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h
View File

@@ -40,6 +40,24 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
units::meters_per_second_squared_t maxLinearAccel,
units::radians_per_second_squared_t maxAngularAccel);
/**
* Constructs a DifferentialDriveAccelerationLimiter.
*
* @param system The differential drive dynamics.
* @param trackwidth The distance between the differential drive's left and
* right wheels.
* @param minLinearAccel The minimum (most negative) linear acceleration.
* @param maxLinearAccel The maximum (most positive) linear acceleration.
* @param maxAngularAccel The maximum angular acceleration.
* @throws std::invalid_argument if minimum linear acceleration is greater
* than maximum linear acceleration
*/
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);
/**
* Returns the next voltage pair subject to acceleraiton constraints.
*
@@ -57,6 +75,7 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter {
private:
LinearSystem<2, 2, 2> m_system;
units::meter_t m_trackwidth;
units::meters_per_second_squared_t m_minLinearAccel;
units::meters_per_second_squared_t m_maxLinearAccel;
units::radians_per_second_squared_t m_maxAngularAccel;
};