WPILibC++  2020.3.2
ArmFeedforward.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 {
19  public:
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>>;
24  using kv_unit =
25  units::compound_unit<units::volts,
26  units::inverse<units::radians_per_second>>;
27  using ka_unit =
28  units::compound_unit<units::volts, units::inverse<Acceleration>>;
29 
30  constexpr ArmFeedforward() = default;
31 
40  constexpr ArmFeedforward(
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) {}
44 
53  units::volt_t Calculate(units::unit_t<Angle> angle,
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;
59  }
60 
61  // Rearranging the main equation from the calculate() method yields the
62  // formulas for the methods below:
63 
76  units::unit_t<Velocity> MaxAchievableVelocity(
77  units::volt_t maxVoltage, units::unit_t<Angle> angle,
78  units::unit_t<Acceleration> acceleration) {
79  // Assume max velocity is positive
80  return (maxVoltage - kS - kCos * units::math::cos(angle) -
81  kA * acceleration) /
82  kV;
83  }
84 
97  units::unit_t<Velocity> MinAchievableVelocity(
98  units::volt_t maxVoltage, units::unit_t<Angle> angle,
99  units::unit_t<Acceleration> acceleration) {
100  // Assume min velocity is negative, ks flips sign
101  return (-maxVoltage + kS - kCos * units::math::cos(angle) -
102  kA * acceleration) /
103  kV;
104  }
105 
118  units::unit_t<Acceleration> MaxAchievableAcceleration(
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) /
123  kA;
124  }
125 
138  units::unit_t<Acceleration> MinAchievableAcceleration(
139  units::volt_t maxVoltage, units::unit_t<Angle> angle,
140  units::unit_t<Velocity> velocity) {
141  return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
142  }
143 
144  units::volt_t kS{0};
145  units::volt_t kCos{0};
146  units::unit_t<kv_unit> kV{0};
147  units::unit_t<ka_unit> kA{0};
148 };
149 } // namespace frc
frc::ArmFeedforward::MinAchievableAcceleration
units::unit_t< Acceleration > MinAchievableAcceleration(units::volt_t maxVoltage, units::unit_t< Angle > angle, units::unit_t< Velocity > velocity)
Calculates the minimum achievable acceleration given a maximum voltage supply, a position,...
Definition: ArmFeedforward.h:138
frc::ArmFeedforward::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))
Creates a new ArmFeedforward with the specified gains.
Definition: ArmFeedforward.h:40
frc::ArmFeedforward::MaxAchievableAcceleration
units::unit_t< Acceleration > MaxAchievableAcceleration(units::volt_t maxVoltage, units::unit_t< Angle > angle, units::unit_t< Velocity > velocity)
Calculates the maximum achievable acceleration given a maximum voltage supply, a position,...
Definition: ArmFeedforward.h:118
frc::ArmFeedforward
A helper class that computes feedforward outputs for a simple arm (modeled as a motor acting against ...
Definition: ArmFeedforward.h:18
frc::ArmFeedforward::MinAchievableVelocity
units::unit_t< Velocity > MinAchievableVelocity(units::volt_t maxVoltage, units::unit_t< Angle > angle, units::unit_t< Acceleration > acceleration)
Calculates the minimum achievable velocity given a maximum voltage supply, a position,...
Definition: ArmFeedforward.h:97
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::ArmFeedforward::Calculate
units::volt_t Calculate(units::unit_t< Angle > angle, 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: ArmFeedforward.h:53
frc::ArmFeedforward::MaxAchievableVelocity
units::unit_t< Velocity > MaxAchievableVelocity(units::volt_t maxVoltage, units::unit_t< Angle > angle, units::unit_t< Acceleration > acceleration)
Calculates the maximum achievable velocity given a maximum voltage supply, a position,...
Definition: ArmFeedforward.h:76