WPILibC++ 2024.3.2
SimpleMotorFeedforward.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <wpi/MathExtras.h>
8
9#include "frc/EigenCore.h"
10
12#include "units/time.h"
13#include "units/voltage.h"
14#include "wpimath/MathShared.h"
15
16namespace frc {
17/**
18 * A helper class that computes feedforward voltages for a simple
19 * permanent-magnet DC motor.
20 */
21template <class Distance>
23 public:
24 using Velocity =
29 using ka_unit =
31
32 /**
33 * Creates a new SimpleMotorFeedforward with the specified gains.
34 *
35 * @param kS The static gain, in volts.
36 * @param kV The velocity gain, in volt seconds per distance.
37 * @param kA The acceleration gain, in volt secondsĀ² per distance.
38 */
40 units::volt_t kS, units::unit_t<kv_unit> kV,
42 : kS(kS), kV(kV), kA(kA) {
43 if (kV.value() < 0) {
45 "kV must be a non-negative number, got {}!", kV.value());
48 }
49 if (kA.value() < 0) {
51 "kA must be a non-negative number, got {}!", kA.value());
54 }
55 }
56
57 /**
58 * Calculates the feedforward from the gains and setpoints.
59 *
60 * @param velocity The velocity setpoint, in distance per second.
61 * @param acceleration The acceleration setpoint, in distance per secondĀ².
62 * @return The computed feedforward, in volts.
63 */
64 constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
65 units::unit_t<Acceleration> acceleration =
67 return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
68 }
69
70 /**
71 * Calculates the feedforward from the gains and setpoints.
72 *
73 * @param currentVelocity The current velocity setpoint, in distance per
74 * second.
75 * @param nextVelocity The next velocity setpoint, in distance per second.
76 * @param dt Time between velocity setpoints in seconds.
77 * @return The computed feedforward, in volts.
78 */
79 units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
80 units::unit_t<Velocity> nextVelocity,
81 units::second_t dt) const {
82 auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
83 LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
84
85 Vectord<1> r{currentVelocity.value()};
86 Vectord<1> nextR{nextVelocity.value()};
87
88 return kS * wpi::sgn(currentVelocity.value()) +
89 units::volt_t{feedforward.Calculate(r, nextR)(0)};
90 }
91
92 // Rearranging the main equation from the calculate() method yields the
93 // formulas for the methods below:
94
95 /**
96 * Calculates the maximum achievable velocity given a maximum voltage supply
97 * and an acceleration. Useful for ensuring that velocity and
98 * acceleration constraints for a trapezoidal profile are simultaneously
99 * achievable - enter the acceleration constraint, and this will give you
100 * a simultaneously-achievable velocity constraint.
101 *
102 * @param maxVoltage The maximum voltage that can be supplied to the motor.
103 * @param acceleration The acceleration of the motor.
104 * @return The maximum possible velocity at the given acceleration.
105 */
107 units::volt_t maxVoltage,
108 units::unit_t<Acceleration> acceleration) const {
109 // Assume max velocity is positive
110 return (maxVoltage - kS - kA * acceleration) / kV;
111 }
112
113 /**
114 * Calculates the minimum achievable velocity given a maximum voltage supply
115 * and an acceleration. Useful for ensuring that velocity and
116 * acceleration constraints for a trapezoidal profile are simultaneously
117 * achievable - enter the acceleration constraint, and this will give you
118 * a simultaneously-achievable velocity constraint.
119 *
120 * @param maxVoltage The maximum voltage that can be supplied to the motor.
121 * @param acceleration The acceleration of the motor.
122 * @return The minimum possible velocity at the given acceleration.
123 */
125 units::volt_t maxVoltage,
126 units::unit_t<Acceleration> acceleration) const {
127 // Assume min velocity is positive, ks flips sign
128 return (-maxVoltage + kS - kA * acceleration) / kV;
129 }
130
131 /**
132 * Calculates the maximum achievable acceleration given a maximum voltage
133 * supply and a velocity. Useful for ensuring that velocity and
134 * acceleration constraints for a trapezoidal profile are simultaneously
135 * achievable - enter the velocity constraint, and this will give you
136 * a simultaneously-achievable acceleration constraint.
137 *
138 * @param maxVoltage The maximum voltage that can be supplied to the motor.
139 * @param velocity The velocity of the motor.
140 * @return The maximum possible acceleration at the given velocity.
141 */
143 units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
144 return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
145 }
146
147 /**
148 * Calculates the minimum achievable acceleration given a maximum voltage
149 * supply and a velocity. Useful for ensuring that velocity and
150 * acceleration constraints for a trapezoidal profile are simultaneously
151 * achievable - enter the velocity constraint, and this will give you
152 * a simultaneously-achievable acceleration constraint.
153 *
154 * @param maxVoltage The maximum voltage that can be supplied to the motor.
155 * @param velocity The velocity of the motor.
156 * @return The minimum possible acceleration at the given velocity.
157 */
159 units::volt_t maxVoltage, units::unit_t<Velocity> velocity) const {
160 return MaxAchievableAcceleration(-maxVoltage, velocity);
161 }
162
163 /** The static gain. */
164 const units::volt_t kS;
165
166 /** The velocity gain. */
168
169 /** The acceleration gain. */
171};
172} // namespace frc
Constructs a plant inversion model-based feedforward from a LinearSystem.
Definition: LinearPlantInversionFeedforward.h:33
A helper class that computes feedforward voltages for a simple permanent-magnet DC motor.
Definition: SimpleMotorFeedforward.h:22
const units::unit_t< kv_unit > kV
The velocity gain.
Definition: SimpleMotorFeedforward.h:167
constexpr units::unit_t< Velocity > MinAchievableVelocity(units::volt_t maxVoltage, units::unit_t< Acceleration > acceleration) const
Calculates the minimum achievable velocity given a maximum voltage supply and an acceleration.
Definition: SimpleMotorFeedforward.h:124
units::volt_t Calculate(units::unit_t< Velocity > currentVelocity, units::unit_t< Velocity > nextVelocity, units::second_t dt) const
Calculates the feedforward from the gains and setpoints.
Definition: SimpleMotorFeedforward.h:79
units::compound_unit< Distance, units::inverse< units::seconds > > Velocity
Definition: SimpleMotorFeedforward.h:25
units::compound_unit< units::volts, units::inverse< Acceleration > > ka_unit
Definition: SimpleMotorFeedforward.h:30
const units::volt_t kS
The static gain.
Definition: SimpleMotorFeedforward.h:164
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:64
constexpr units::unit_t< Acceleration > MaxAchievableAcceleration(units::volt_t maxVoltage, units::unit_t< Velocity > velocity) const
Calculates the maximum achievable acceleration given a maximum voltage supply and a velocity.
Definition: SimpleMotorFeedforward.h:142
units::compound_unit< units::volts, units::inverse< Velocity > > kv_unit
Definition: SimpleMotorFeedforward.h:28
units::compound_unit< Velocity, units::inverse< units::seconds > > Acceleration
Definition: SimpleMotorFeedforward.h:27
constexpr units::unit_t< Acceleration > MinAchievableAcceleration(units::volt_t maxVoltage, units::unit_t< Velocity > velocity) const
Calculates the minimum achievable acceleration given a maximum voltage supply and a velocity.
Definition: SimpleMotorFeedforward.h:158
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:39
constexpr units::unit_t< Velocity > MaxAchievableVelocity(units::volt_t maxVoltage, units::unit_t< Acceleration > acceleration) const
Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
Definition: SimpleMotorFeedforward.h:106
const units::unit_t< ka_unit > kA
The acceleration gain.
Definition: SimpleMotorFeedforward.h:170
constexpr underlying_type value() const noexcept
unit value
Definition: base.h:2107
static void ReportError(const S &format, Args &&... args)
Definition: MathShared.h:60
static void ReportWarning(const S &format, Args &&... args)
Definition: MathShared.h:69
typename units::detail::compound_impl< U, Us... >::type compound_unit
Represents a unit type made up from other units.
Definition: base.h:1434
Definition: AprilTagPoseEstimator.h:15
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
constexpr int sgn(T val)
Definition: MathExtras.h:624