WPILibC++  2020.3.2
SimpleMotorFeedforward.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  constexpr SimpleMotorFeedforward() = default;
30 
39  units::volt_t kS, units::unit_t<kv_unit> kV,
40  units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
41  : kS(kS), kV(kV), kA(kA) {}
42 
50  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
51  units::unit_t<Acceleration> acceleration =
52  units::unit_t<Acceleration>(0)) const {
53  return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
54  }
55 
56  // Rearranging the main equation from the calculate() method yields the
57  // formulas for the methods below:
58 
70  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
71  units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
72  // Assume max velocity is positive
73  return (maxVoltage - kS - kA * acceleration) / kV;
74  }
75 
87  constexpr units::unit_t<Velocity> MinAchievableVelocity(
88  units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
89  // Assume min velocity is positive, ks flips sign
90  return (-maxVoltage + kS - kA * acceleration) / kV;
91  }
92 
104  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
105  units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
106  return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
107  }
108 
120  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
121  units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
122  return MaxAchievableAcceleration(-maxVoltage, velocity);
123  }
124 
125  units::volt_t kS{0};
126  units::unit_t<kv_unit> kV{0};
127  units::unit_t<ka_unit> kA{0};
128 };
129 } // namespace frc
frc::SimpleMotorFeedforward::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: SimpleMotorFeedforward.h:70
frc::SimpleMotorFeedforward::SimpleMotorFeedforward
constexpr SimpleMotorFeedforward(units::volt_t kS, units::unit_t< kv_unit > kV, units::unit_t< ka_unit > kA=units::unit_t< ka_unit >(0))
Creates a new SimpleMotorFeedforward with the specified gains.
Definition: SimpleMotorFeedforward.h:38
frc::SimpleMotorFeedforward
A helper class that computes feedforward voltages for a simple permanent-magnet DC motor.
Definition: SimpleMotorFeedforward.h:19
frc::SimpleMotorFeedforward::Calculate
constexpr units::volt_t Calculate(units::unit_t< Velocity > velocity, units::unit_t< Acceleration > acceleration=units::unit_t< Acceleration >(0)) const
Calculates the feedforward from the gains and setpoints.
Definition: SimpleMotorFeedforward.h:50
frc::SimpleMotorFeedforward::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: SimpleMotorFeedforward.h:104
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::SimpleMotorFeedforward::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: SimpleMotorFeedforward.h:120
frc::SimpleMotorFeedforward::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: SimpleMotorFeedforward.h:87