10 #include <units/units.h>
11 #include <wpi/MathExtras.h>
20 using Angle = units::radians;
21 using Velocity = units::radians_per_second;
22 using Acceleration = units::compound_unit<units::radians_per_second,
23 units::inverse<units::second>>;
25 units::compound_unit<units::volts,
26 units::inverse<units::radians_per_second>>;
28 units::compound_unit<units::volts, units::inverse<Acceleration>>;
41 units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
42 units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
43 : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
54 units::unit_t<Velocity> velocity,
55 units::unit_t<Acceleration> acceleration =
56 units::unit_t<Acceleration>(0))
const {
57 return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
58 kV * velocity + kA * acceleration;
77 units::volt_t maxVoltage, units::unit_t<Angle> angle,
78 units::unit_t<Acceleration> acceleration) {
80 return (maxVoltage - kS - kCos * units::math::cos(angle) -
98 units::volt_t maxVoltage, units::unit_t<Angle> angle,
99 units::unit_t<Acceleration> acceleration) {
101 return (-maxVoltage + kS - kCos * units::math::cos(angle) -
119 units::volt_t maxVoltage, units::unit_t<Angle> angle,
120 units::unit_t<Velocity> velocity) {
121 return (maxVoltage - kS * wpi::sgn(velocity) -
122 kCos * units::math::cos(angle) - kV * velocity) /
139 units::volt_t maxVoltage, units::unit_t<Angle> angle,
140 units::unit_t<Velocity> velocity) {
145 units::volt_t kCos{0};
146 units::unit_t<kv_unit> kV{0};
147 units::unit_t<ka_unit> kA{0};