WPILibC++  2020.3.2
ElevatorFeedforward.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <units/units.h>
11 #include <wpi/MathExtras.h>
12 
13 namespace frc {
18 template <class Distance>
20  public:
21  using Velocity =
22  units::compound_unit<Distance, units::inverse<units::seconds>>;
23  using Acceleration =
24  units::compound_unit<Velocity, units::inverse<units::seconds>>;
25  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
26  using ka_unit =
27  units::compound_unit<units::volts, units::inverse<Acceleration>>;
28 
29  ElevatorFeedforward() = default;
30 
40  units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
41  units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
42  : kS(kS), kG(kG), kV(kV), kA(kA) {}
43 
51  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
52  units::unit_t<Acceleration> acceleration =
53  units::unit_t<Acceleration>(0)) {
54  return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
55  }
56 
57  // Rearranging the main equation from the calculate() method yields the
58  // formulas for the methods below:
59 
71  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
72  units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
73  // Assume max velocity is positive
74  return (maxVoltage - kS - kG - kA * acceleration) / kV;
75  }
76 
88  constexpr units::unit_t<Velocity> MinAchievableVelocity(
89  units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
90  // Assume min velocity is negative, ks flips sign
91  return (-maxVoltage + kS - kG - kA * acceleration) / kV;
92  }
93 
105  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
106  units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
107  return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
108  }
109 
121  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
122  units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
123  return MaxAchievableAcceleration(-maxVoltage, velocity);
124  }
125 
126  units::volt_t kS{0};
127  units::volt_t kG{0};
128  units::unit_t<kv_unit> kV{0};
129  units::unit_t<ka_unit> kA{0};
130 };
131 } // namespace frc
frc::ElevatorFeedforward
A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting aga...
Definition: ElevatorFeedforward.h:19
frc::ElevatorFeedforward::MinAchievableAcceleration
constexpr units::unit_t< Acceleration > MinAchievableAcceleration(units::volt_t maxVoltage, units::unit_t< Velocity > velocity)
Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
Definition: ElevatorFeedforward.h:121
frc::ElevatorFeedforward::MaxAchievableAcceleration
constexpr units::unit_t< Acceleration > MaxAchievableAcceleration(units::volt_t maxVoltage, units::unit_t< Velocity > velocity)
Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
Definition: ElevatorFeedforward.h:105
frc::ElevatorFeedforward::Calculate
constexpr units::volt_t Calculate(units::unit_t< Velocity > velocity, units::unit_t< Acceleration > acceleration=units::unit_t< Acceleration >(0))
Calculates the feedforward from the gains and setpoints.
Definition: ElevatorFeedforward.h:51
frc::ElevatorFeedforward::MinAchievableVelocity
constexpr units::unit_t< Velocity > MinAchievableVelocity(units::volt_t maxVoltage, units::unit_t< Acceleration > acceleration)
Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
Definition: ElevatorFeedforward.h:88
frc::ElevatorFeedforward::MaxAchievableVelocity
constexpr units::unit_t< Velocity > MaxAchievableVelocity(units::volt_t maxVoltage, units::unit_t< Acceleration > acceleration)
Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
Definition: ElevatorFeedforward.h:71
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::ElevatorFeedforward::ElevatorFeedforward
constexpr ElevatorFeedforward(units::volt_t kS, units::volt_t kG, units::unit_t< kv_unit > kV, units::unit_t< ka_unit > kA=units::unit_t< ka_unit >(0))
Creates a new ElevatorFeedforward with the specified gains.
Definition: ElevatorFeedforward.h:39