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

Add DifferentialDrive voltage constraint (#2075)

Browse Source
This commit is contained in:
Oblarg
2019-11-22 00:43:02 -05:00
committed by Peter Johnson
parent e0bc97f66b
commit 21e957ee42
17 changed files with 868 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
View File

@@ -40,7 +40,7 @@ class ArmFeedforward {
constexpr ArmFeedforward(
units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: m_kS(kS), m_kCos(kCos), m_kV(kV), m_kA(kA) {}
: kS(kS), kCos(kCos), kV(kV), kA(kA) {}
/**
* Calculates the feedforward from the gains and setpoints.
@@ -54,14 +54,94 @@ class ArmFeedforward {
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
return m_kS * wpi::sgn(velocity) + m_kCos * units::math::cos(angle) +
m_kV * velocity + m_kA * acceleration;
return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
kV * velocity + kA * acceleration;
}
private:
units::volt_t m_kS{0};
units::volt_t m_kCos{0};
units::unit_t<kv_unit> m_kV{0};
units::unit_t<ka_unit> m_kA{0};
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
/**
* Calculates the maximum achievable velocity given a maximum voltage supply,
* a position, and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param acceleration The acceleration of the arm.
* @return The maximum possible velocity at the given acceleration and angle.
*/
units::unit_t<Velocity> MaxAchievableVelocity(
units::volt_t maxVoltage, Angle angle,
units::unit_t<Acceleration> acceleration) {
// Assume max velocity is positive
return (maxVoltage - kS - kCos * units::math::cos(angle) -
kA * acceleration) /
kV;
}
/**
* Calculates the minimum achievable velocity given a maximum voltage supply,
* a position, and an acceleration. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the acceleration constraint, and this will give you
* a simultaneously-achievable velocity constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param acceleration The acceleration of the arm.
* @return The minimum possible velocity at the given acceleration and angle.
*/
units::unit_t<Velocity> MinAchievableVelocity(
units::volt_t maxVoltage, Angle angle,
units::unit_t<Acceleration> acceleration) {
// Assume min velocity is negative, ks flips sign
return (-maxVoltage + kS - kCos * units::math::cos(angle) -
kA * acceleration) /
kV;
}
/**
* Calculates the maximum achievable acceleration given a maximum voltage
* supply, a position, and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param velocity The velocity of the arm.
* @return The maximum possible acceleration at the given velocity and angle.
*/
units::unit_t<Acceleration> MaxAchievableAcceleration(
units::volt_t maxVoltage, Angle angle, units::unit_t<Velocity> velocity) {
return (maxVoltage - kS * wpi::sgn(velocity) -
kCos * units::math::cos(angle) - kV * velocity) /
kA;
}
/**
* Calculates the minimum achievable acceleration given a maximum voltage
* supply, a position, and a velocity. Useful for ensuring that velocity and
* acceleration constraints for a trapezoidal profile are simultaneously
* achievable - enter the velocity constraint, and this will give you
* a simultaneously-achievable acceleration constraint.
*
* @param maxVoltage The maximum voltage that can be supplied to the arm.
* @param angle The angle of the arm
* @param velocity The velocity of the arm.
* @return The minimum possible acceleration at the given velocity and angle.
*/
units::unit_t<Acceleration> MinAchievableAcceleration(
units::volt_t maxVoltage, Angle angle, units::unit_t<Velocity> velocity) {
return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
}
units::volt_t kS{0};
units::volt_t kCos{0};
units::unit_t<kv_unit> kV{0};
units::unit_t<ka_unit> kA{0};
};
} // namespace frc