Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-07-16 20:23:11 -04:00
committed by GitHub
parent 5f261a88af
commit 30c7632ab8
31 changed files with 540 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.controller.HolonomicDriveController;
@@ -17,6 +19,9 @@ import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Consumer;
import java.util.function.Supplier;
@@ -55,8 +60,22 @@ public class MecanumControllerCommand extends Command {
private final Supplier<MecanumDriveWheelSpeeds> m_currentWheelSpeeds;
private final Consumer<MecanumDriveMotorVoltages> m_outputDriveVoltages;
private final Consumer<MecanumDriveWheelSpeeds> m_outputWheelSpeeds;
private MecanumDriveWheelSpeeds m_prevSpeeds;
private double m_prevTime;
private final MutableMeasure<Velocity<Distance>> m_prevFrontLeftSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_prevRearLeftSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_prevFrontRightSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_prevRearRightSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_frontLeftSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_rearLeftSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_frontRightSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_rearRightSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
/**
* Constructs a new MecanumControllerCommand that when executed will follow the provided
@@ -331,16 +350,20 @@ public class MecanumControllerCommand extends Command {
var initialYVelocity =
initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
m_prevSpeeds =
MecanumDriveWheelSpeeds prevSpeeds =
m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
m_prevFrontLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontLeftMetersPerSecond);
m_prevRearLeftSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearLeftMetersPerSecond);
m_prevFrontRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.frontRightMetersPerSecond);
m_prevRearRightSpeedSetpoint.mut_setMagnitude(prevSpeeds.rearRightMetersPerSecond);
m_timer.restart();
}
@Override
public void execute() {
double curTime = m_timer.get();
double dt = curTime - m_prevTime;
var desiredState = m_trajectory.sample(curTime);
@@ -350,10 +373,10 @@ public class MecanumControllerCommand extends Command {
targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
m_frontLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontLeftMetersPerSecond);
m_rearLeftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearLeftMetersPerSecond);
m_frontRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.frontRightMetersPerSecond);
m_rearRightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rearRightMetersPerSecond);
double frontLeftOutput;
double rearLeftOutput;
@@ -362,44 +385,42 @@ public class MecanumControllerCommand extends Command {
if (m_usePID) {
final double frontLeftFeedforward =
m_feedforward.calculate(
frontLeftSpeedSetpoint,
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, m_frontLeftSpeedSetpoint).in(Volts);
final double rearLeftFeedforward =
m_feedforward.calculate(
rearLeftSpeedSetpoint,
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, m_rearLeftSpeedSetpoint).in(Volts);
final double frontRightFeedforward =
m_feedforward.calculate(
frontRightSpeedSetpoint,
(frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
m_feedforward
.calculate(m_prevFrontRightSpeedSetpoint, m_frontRightSpeedSetpoint)
.in(Volts);
final double rearRightFeedforward =
m_feedforward.calculate(
rearRightSpeedSetpoint,
(rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
m_feedforward.calculate(m_prevRearRightSpeedSetpoint, m_rearRightSpeedSetpoint).in(Volts);
frontLeftOutput =
frontLeftFeedforward
+ m_frontLeftController.calculate(
m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
m_frontLeftSpeedSetpoint.in(MetersPerSecond));
rearLeftOutput =
rearLeftFeedforward
+ m_rearLeftController.calculate(
m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
m_rearLeftSpeedSetpoint.in(MetersPerSecond));
frontRightOutput =
frontRightFeedforward
+ m_frontRightController.calculate(
m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
m_currentWheelSpeeds.get().frontRightMetersPerSecond,
m_frontRightSpeedSetpoint.in(MetersPerSecond));
rearRightOutput =
rearRightFeedforward
+ m_rearRightController.calculate(
m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
m_currentWheelSpeeds.get().rearRightMetersPerSecond,
m_rearRightSpeedSetpoint.in(MetersPerSecond));
m_outputDriveVoltages.accept(
new MecanumDriveMotorVoltages(
@@ -408,14 +429,11 @@ public class MecanumControllerCommand extends Command {
} else {
m_outputWheelSpeeds.accept(
new MecanumDriveWheelSpeeds(
frontLeftSpeedSetpoint,
frontRightSpeedSetpoint,
rearLeftSpeedSetpoint,
rearRightSpeedSetpoint));
m_frontLeftSpeedSetpoint.in(MetersPerSecond),
m_frontRightSpeedSetpoint.in(MetersPerSecond),
m_rearLeftSpeedSetpoint.in(MetersPerSecond),
m_rearRightSpeedSetpoint.in(MetersPerSecond)));
}
m_prevTime = curTime;
m_prevSpeeds = targetWheelSpeeds;
}
@Override

25
wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.wpilibj2.command;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
import edu.wpi.first.math.controller.PIDController;
@@ -14,6 +16,9 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.BiConsumer;
@@ -46,6 +51,14 @@ public class RamseteCommand extends Command {
private final PIDController m_rightController;
private final BiConsumer<Double, Double> m_output;
private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds();
private final MutableMeasure<Velocity<Distance>> m_prevLeftSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_prevRightSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_leftSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private final MutableMeasure<Velocity<Distance>> m_rightSpeedSetpoint =
MutableMeasure.zero(MetersPerSecond);
private double m_prevTime;
/**
@@ -149,6 +162,8 @@ public class RamseteCommand extends Command {
initialState.velocityMetersPerSecond,
0,
initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond));
m_prevLeftSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.leftMetersPerSecond);
m_prevRightSpeedSetpoint.mut_setMagnitude(m_prevSpeeds.rightMetersPerSecond);
m_timer.restart();
if (m_usePID) {
m_leftController.reset();
@@ -159,7 +174,6 @@ public class RamseteCommand extends Command {
@Override
public void execute() {
double curTime = m_timer.get();
double dt = curTime - m_prevTime;
if (m_prevTime < 0) {
m_output.accept(0.0, 0.0);
@@ -174,17 +188,18 @@ public class RamseteCommand extends Command {
var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
m_leftSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.leftMetersPerSecond);
m_rightSpeedSetpoint.mut_setMagnitude(targetWheelSpeeds.rightMetersPerSecond);
double leftOutput;
double rightOutput;
if (m_usePID) {
double leftFeedforward =
m_feedforward.calculate(
leftSpeedSetpoint, (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
m_feedforward.calculate(m_prevLeftSpeedSetpoint, m_leftSpeedSetpoint).in(Volts);
double rightFeedforward =
m_feedforward.calculate(
rightSpeedSetpoint, (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
m_feedforward.calculate(m_prevRightSpeedSetpoint, m_rightSpeedSetpoint).in(Volts);
leftOutput =
leftFeedforward

19
wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
View File

@@ -159,7 +159,6 @@ void MecanumControllerCommand::Initialize() {
void MecanumControllerCommand::Execute() {
auto curTime = m_timer.Get();
auto dt = curTime - m_prevTime;
auto m_desiredState = m_trajectory.Sample(curTime);
@@ -175,21 +174,17 @@ void MecanumControllerCommand::Execute() {
auto rearRightSpeedSetpoint = targetWheelSpeeds.rearRight;
if (m_usePID) {
auto frontLeftFeedforward = m_feedforward.Calculate(
frontLeftSpeedSetpoint,
(frontLeftSpeedSetpoint - m_prevSpeeds.frontLeft) / dt);
auto frontLeftFeedforward =
m_feedforward.Calculate(m_prevSpeeds.frontLeft, frontLeftSpeedSetpoint);
auto rearLeftFeedforward = m_feedforward.Calculate(
rearLeftSpeedSetpoint,
(rearLeftSpeedSetpoint - m_prevSpeeds.rearLeft) / dt);
auto rearLeftFeedforward =
m_feedforward.Calculate(m_prevSpeeds.rearLeft, rearLeftSpeedSetpoint);
auto frontRightFeedforward = m_feedforward.Calculate(
frontRightSpeedSetpoint,
(frontRightSpeedSetpoint - m_prevSpeeds.frontRight) / dt);
m_prevSpeeds.frontRight, frontRightSpeedSetpoint);
auto rearRightFeedforward = m_feedforward.Calculate(
rearRightSpeedSetpoint,
(rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
auto rearRightFeedforward =
m_feedforward.Calculate(m_prevSpeeds.rearRight, rearRightSpeedSetpoint);
auto frontLeftOutput = units::volt_t{m_frontLeftController->Calculate(
m_currentWheelSpeeds().frontLeft.value(),

11
wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
View File

@@ -68,7 +68,6 @@ void RamseteCommand::Initialize() {
void RamseteCommand::Execute() {
auto curTime = m_timer.Get();
auto dt = curTime - m_prevTime;
if (m_prevTime < 0_s) {
if (m_usePID) {
@@ -85,13 +84,11 @@ void RamseteCommand::Execute() {
m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
if (m_usePID) {
auto leftFeedforward = m_feedforward.Calculate(
targetWheelSpeeds.left,
(targetWheelSpeeds.left - m_prevSpeeds.left) / dt);
auto leftFeedforward =
m_feedforward.Calculate(m_prevSpeeds.left, targetWheelSpeeds.left);
auto rightFeedforward = m_feedforward.Calculate(
targetWheelSpeeds.right,
(targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
auto rightFeedforward =
m_feedforward.Calculate(m_prevSpeeds.right, targetWheelSpeeds.right);
auto leftOutput =
units::volt_t{m_leftController->Calculate(

3
wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp
View File

@@ -40,8 +40,7 @@ class Robot : public frc::TimedRobot {
m_motor.SetSetpoint(
ExampleSmartMotorController::PIDMode::kPosition,
m_setpoint.position.value(),
m_feedforward.Calculate(m_setpoint.velocity, next.velocity, kDt) /
12_V);
m_feedforward.Calculate(m_setpoint.velocity, next.velocity) / 12_V);
m_setpoint = next;
}

7
wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp
View File

@@ -54,9 +54,10 @@ class Robot : public frc::TimedRobot {
// accelerate the shooter wheel
.IfHigh([&shooter = m_shooter, &controller = m_controller, &ff = m_ff,
&encoder = m_shooterEncoder] {
shooter.SetVoltage(units::volt_t{controller.Calculate(
encoder.GetRate(), SHOT_VELOCITY.value())} +
ff.Calculate(SHOT_VELOCITY));
shooter.SetVoltage(
units::volt_t{controller.Calculate(encoder.GetRate(),
SHOT_VELOCITY.value())} +
ff.Calculate(units::radians_per_second_t{SHOT_VELOCITY}));
});
// if not, stop
(!shootTrigger).IfHigh([&shooter = m_shooter] { shooter.Set(0.0); });

1
wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp
View File

@@ -11,6 +11,7 @@
#include <frc/simulation/EncoderSim.h>
#include <frc/simulation/FlywheelSim.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <frc/system/plant/LinearSystemId.h>
#include <units/moment_of_inertia.h>
/**

1
wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
View File

@@ -5,6 +5,7 @@
#include "subsystems/ShooterSubsystem.h"
#include <frc/controller/PIDController.h>
#include <units/angular_velocity.h>
#include "Constants.h"

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.differentialdrivebot;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -79,8 +82,10 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);
final double rightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.ComputerVisionUtil;
@@ -139,8 +142,10 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);
final double rightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts);
final double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);

7
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -75,11 +78,11 @@ public class DriveSubsystem extends SubsystemBase {
m_leftLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
left.position,
m_feedforward.calculate(left.velocity));
m_feedforward.calculate(MetersPerSecond.of(left.velocity)).in(Volts));
m_rightLeader.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
right.position,
m_feedforward.calculate(right.velocity));
m_feedforward.calculate(MetersPerSecond.of(right.velocity)).in(Volts));
}
/**

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.elevatorexponentialprofile;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.ExponentialProfile;
import edu.wpi.first.wpilibj.Joystick;
@@ -46,7 +49,7 @@ public class Robot extends TimedRobot {
m_motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity, next.velocity, 0.02) / 12.0);
m_feedforward.calculate(RadiansPerSecond.of(next.velocity)).in(Volts) / 12.0);
m_setpoint = next;
}

5
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Joystick;
@@ -46,6 +49,6 @@ public class Robot extends TimedRobot {
m_motor.setSetpoint(
ExampleSmartMotorController.PIDMode.kPosition,
m_setpoint.position,
m_feedforward.calculate(m_setpoint.velocity) / 12.0);
m_feedforward.calculate(MetersPerSecond.of(m_setpoint.velocity)).in(Volts) / 12.0);
}
}

13
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.eventloop;
import static edu.wpi.first.units.Units.RPM;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
@@ -61,10 +64,12 @@ public class Robot extends TimedRobot {
shootTrigger
// accelerate the shooter wheel
.ifHigh(
() ->
m_shooter.setVoltage(
m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY)
+ m_ff.calculate(SHOT_VELOCITY)));
() -> {
m_shooter.setVoltage(
m_controller.calculate(m_shooterEncoder.getRate(), SHOT_VELOCITY)
+ m_ff.calculate(RPM.of(SHOT_VELOCITY)).in(Volts));
});
// if not, stop
shootTrigger.negate().ifHigh(m_shooter::stopMotor);

6
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.BangBangController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.numbers.N1;
@@ -87,7 +90,8 @@ public class Robot extends TimedRobot {
// Controls a motor with the output of the BangBang controller and a
// feedforward. The feedforward is reduced slightly to avoid overspeeding
// the shooter.
m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint));
m_flywheelMotor.setVoltage(
bangOutput + 0.9 * m_feedforward.calculate(RadiansPerSecond.of(setpoint)).in(Volts));
}
/** Update our simulation. This should be run every robot loop in simulation. */

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.Encoder;
@@ -33,7 +36,11 @@ public class ShooterSubsystem extends PIDSubsystem {
@Override
public void useOutput(double output, double setpoint) {
m_shooterMotor.setVoltage(output + m_shooterFeedforward.calculate(setpoint));
m_shooterMotor.setVoltage(
output
+ m_shooterFeedforward
.calculate(RadiansPerSecond.of(ShooterConstants.kShooterTargetRPS))
.in(Volts));
}
@Override

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.mecanumbot;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Translation2d;
@@ -95,10 +98,14 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
final double frontLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts);
final double frontRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts);
final double backLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts);
final double backRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(

15
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -107,10 +110,14 @@ public class Drivetrain {
* @param speeds The desired wheel speeds.
*/
public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
final double frontLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontLeftMetersPerSecond)).in(Volts);
final double frontRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.frontRightMetersPerSecond)).in(Volts);
final double backLeftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearLeftMetersPerSecond)).in(Volts);
final double backRightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rearRightMetersPerSecond)).in(Volts);
final double frontLeftOutput =
m_frontLeftPIDController.calculate(

16
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.wpilibj.examples.rapidreactcommandbot.subsystems;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static edu.wpi.first.wpilibj2.command.Commands.parallel;
import static edu.wpi.first.wpilibj2.command.Commands.waitUntil;
@@ -54,11 +56,15 @@ public class Shooter extends SubsystemBase {
return parallel(
// Run the shooter flywheel at the desired setpoint using feedforward and feedback
run(
() ->
m_shooterMotor.set(
m_shooterFeedforward.calculate(setpointRotationsPerSecond)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpointRotationsPerSecond))),
() -> {
m_shooterMotor.set(
m_shooterFeedforward
.calculate(RotationsPerSecond.of(setpointRotationsPerSecond))
.in(Volts)
+ m_shooterFeedback.calculate(
m_shooterEncoder.getRate(), setpointRotationsPerSecond));
}),
// Wait until the shooter has reached the setpoint, and then run the feeder
waitUntil(m_shooterFeedback::atSetpoint).andThen(() -> m_feederMotor.set(1)))
.withName("Shoot");

9
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
@@ -94,8 +97,10 @@ public class Drivetrain {
/** Sets speeds to the drivetrain motors. */
public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
final double leftFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.leftMetersPerSecond)).in(Volts);
final double rightFeedforward =
m_feedforward.calculate(MetersPerSecond.of(speeds.rightMetersPerSecond)).in(Volts);
double leftOutput =
m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
double rightOutput =

12
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.wpilibj.examples.swervebot;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -119,17 +123,21 @@ public class SwerveModule {
state.speedMetersPerSecond *= state.angle.minus(encoderRotation).getCos();
// Calculate the drive output from the drive PID controller.
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
final double driveFeedforward =
m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
m_turnFeedforward
.calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity))
.in(Volts);
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);

11
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
View File

@@ -4,6 +4,10 @@
package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
@@ -122,14 +126,17 @@ public class SwerveModule {
final double driveOutput =
m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
final double driveFeedforward =
m_driveFeedforward.calculate(MetersPerSecond.of(state.speedMetersPerSecond)).in(Volts);
// Calculate the turning motor output from the turning PID controller.
final double turnOutput =
m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
final double turnFeedforward =
m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
m_turnFeedforward
.calculate(RadiansPerSecond.of(m_turningPIDController.getSetpoint().velocity))
.in(Volts);
m_driveMotor.setVoltage(driveOutput + driveFeedforward);
m_turningMotor.setVoltage(turnOutput + turnFeedforward);

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java
View File

@@ -98,7 +98,9 @@ public class Shooter extends SubsystemBase {
return run(() -> {
m_shooterMotor.setVoltage(
m_shooterFeedback.calculate(m_shooterEncoder.getRate(), shooterSpeed.getAsDouble())
+ m_shooterFeedforward.calculate(shooterSpeed.getAsDouble()));
+ m_shooterFeedforward
.calculate(RotationsPerSecond.of(shooterSpeed.getAsDouble()))
.in(Volts));
m_feederMotor.set(ShooterConstants.kFeederSpeed);
})
.finallyDo(

157
wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
View File

@@ -4,9 +4,12 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.system.plant.LinearSystemId;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Unit;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
public class SimpleMotorFeedforward {
@@ -19,17 +22,22 @@ public class SimpleMotorFeedforward {
/** The acceleration gain. */
public final double ka;
/** The period. */
private double m_dt;
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values will
* dictate units of the computed feedforward.
* Creates a new SimpleMotorFeedforward with the specified gains and period. Units of the gain
* values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @param dtSeconds The period in seconds.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
* @throws IllegalArgumentException for period &le; zero.
*/
public SimpleMotorFeedforward(double ks, double kv, double ka) {
public SimpleMotorFeedforward(double ks, double kv, double ka, double dtSeconds) {
this.ks = ks;
this.kv = kv;
this.ka = ka;
@@ -39,17 +47,37 @@ public class SimpleMotorFeedforward {
if (ka < 0.0) {
throw new IllegalArgumentException("ka must be a non-negative number, got " + ka + "!");
}
if (dtSeconds <= 0.0) {
throw new IllegalArgumentException(
"period must be a positive number, got " + dtSeconds + "!");
}
m_dt = dtSeconds;
}
/**
* Creates a new SimpleMotorFeedforward with the specified gains and period. The period is
* defaulted to 20 ms. Units of the gain values will dictate units of the computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
* @param ka The acceleration gain.
* @throws IllegalArgumentException for kv &lt; zero.
* @throws IllegalArgumentException for ka &lt; zero.
*/
public SimpleMotorFeedforward(double ks, double kv, double ka) {
this(ks, kv, ka, 0.020);
}
/**
* Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
* to zero. Units of the gain values will dictate units of the computed feedforward.
* to zero. The period is defaulted to 20 ms. Units of the gain values will dictate units of the
* computed feedforward.
*
* @param ks The static gain.
* @param kv The velocity gain.
*/
public SimpleMotorFeedforward(double ks, double kv) {
this(ks, kv, 0);
this(ks, kv, 0, 0.020);
}
/**
@@ -58,43 +86,114 @@ public class SimpleMotorFeedforward {
* @param velocity The velocity setpoint.
* @param acceleration The acceleration setpoint.
* @return The computed feedforward.
* @deprecated Use the current/next velocity overload instead.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity, double acceleration) {
return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @param dtSeconds Time between velocity setpoints in seconds.
* @return The computed feedforward.
*/
public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
var r = MatBuilder.fill(Nat.N1(), Nat.N1(), currentVelocity);
var nextR = MatBuilder.fill(Nat.N1(), Nat.N1(), nextVelocity);
return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
}
// Rearranging the main equation from the calculate() method yields the
// formulas for the methods below:
/**
* Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
* zero).
*
* @param velocity The velocity setpoint.
* @return The computed feedforward.
* @deprecated Use the current/next velocity overload instead.
*/
@SuppressWarnings("removal")
@Deprecated(forRemoval = true, since = "2025")
public double calculate(double velocity) {
return calculate(velocity, 0);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control when the
* setpoint does not change.
*
* @param <U> The velocity parameter either as distance or angle.
* @param setpoint The velocity setpoint.
* @return The computed feedforward.
*/
public <U extends Unit<U>> Measure<Voltage> calculate(Measure<Velocity<U>> setpoint) {
return calculate(setpoint, setpoint);
}
/**
* Calculates the feedforward from the gains and setpoints assuming discrete control.
*
* @param <U> The velocity parameter either as distance or angle.
* @param currentVelocity The current velocity setpoint.
* @param nextVelocity The next velocity setpoint.
* @return The computed feedforward.
*/
public <U extends Unit<U>> Measure<Voltage> calculate(
Measure<Velocity<U>> currentVelocity, Measure<Velocity<U>> nextVelocity) {
if (ka == 0.0) {
// Given the following discrete feedforward model
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
//
// We want the feedforward model when kₐ = 0.
//
// Simplify A.
//
// A = −kᵥ/kₐ
//
// As kₐ approaches zero, A approaches -∞.
//
// A = −∞
//
// Simplify A_d.
//
// A_d = eᴬᵀ
// A_d = exp(−∞)
// A_d = 0
//
// Simplify B_d.
//
// B_d = A⁻¹(eᴬᵀ - I)B
// B_d = A⁻¹((0) - I)B
// B_d = A⁻¹(-I)B
// B_d = -A⁻¹B
// B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = kₐ/kᵥ(1/kₐ)
// B_d = 1/kᵥ
//
// Substitute these into the feedforward equation.
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
// uₖ = (1/kᵥ)⁺(rₖ₊₁ − (0) rₖ)
// uₖ = kᵥrₖ₊₁
return Volts.of(ks * Math.signum(nextVelocity.magnitude()) + kv * nextVelocity.magnitude());
} else {
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
double A = -kv / ka;
double B = 1.0 / ka;
double A_d = Math.exp(A * m_dt);
double B_d = 1.0 / A * (A_d - 1.0) * B;
return Volts.of(
ks * Math.signum(currentVelocity.magnitude())
+ 1.0 / B_d * (nextVelocity.magnitude() - A_d * currentVelocity.magnitude()));
}
}
/**
* Calculates the maximum achievable velocity given a maximum voltage supply and an acceleration.
* Useful for ensuring that velocity and acceleration constraints for a trapezoidal profile are

150
wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
View File

@@ -4,16 +4,15 @@
#pragma once
#include <gcem.hpp>
#include <wpi/MathExtras.h>
#include "frc/EigenCore.h"
#include "frc/controller/LinearPlantInversionFeedforward.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/time.h"
#include "units/voltage.h"
#include "wpimath/MathShared.h"
namespace frc {
/**
* A helper class that computes feedforward voltages for a simple
* permanent-magnet DC motor.
@@ -35,22 +34,40 @@ class SimpleMotorFeedforward {
* @param kS The static gain, in volts.
* @param kV The velocity gain, in volt seconds per distance.
* @param kA The acceleration gain, in volt seconds² per distance.
* @param dt The period in seconds.
*/
constexpr SimpleMotorFeedforward(
units::volt_t kS, units::unit_t<kv_unit> kV,
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
: kS(kS), kV(kV), kA(kA) {
if (kV.value() < 0) {
units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0),
units::second_t dt = 20_ms)
: kS(kS),
kV([&] {
if (kV.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
return units::unit_t<kv_unit>{0};
} else {
return kV;
}
}()),
kA([&] {
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0.");
return units::unit_t<ka_unit>{0};
} else {
return kA;
}
}()) {
if (dt <= 0_ms) {
wpi::math::MathSharedStore::ReportError(
"kV must be a non-negative number, got {}!", kV.value());
kV = units::unit_t<kv_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0.");
}
if (kA.value() < 0) {
wpi::math::MathSharedStore::ReportError(
"kA must be a non-negative number, got {}!", kA.value());
kA = units::unit_t<ka_unit>{0};
wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;");
"period must be a positive number, got {}!", dt.value());
m_dt = 20_ms;
wpi::math::MathSharedStore::ReportWarning("period defaulted to 20 ms.");
} else {
m_dt = dt;
}
}
@@ -60,33 +77,101 @@ class SimpleMotorFeedforward {
* @param velocity The velocity setpoint, in distance per second.
* @param acceleration The acceleration setpoint, in distance per second².
* @return The computed feedforward, in volts.
* @deprecated Use the current/next velocity overload instead.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration =
units::unit_t<Acceleration>(0)) const {
[[deprecated("Use the current/next velocity overload instead.")]]
constexpr units::volt_t Calculate(
units::unit_t<Velocity> velocity,
units::unit_t<Acceleration> acceleration) const {
return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
}
/**
* Calculates the feedforward from the gains and setpoint.
* Use this method when the setpoint does not change.
*
* @param setpoint The velocity setpoint, in distance per
* second.
* @return The computed feedforward, in volts.
*/
constexpr units::volt_t Calculate(units::unit_t<Velocity> setpoint) const {
return Calculate(setpoint, setpoint);
}
/**
* Calculates the feedforward from the gains and setpoints.
*
* @param currentVelocity The current velocity setpoint, in distance per
* second.
* @param nextVelocity The next velocity setpoint, in distance per second.
* @param dt Time between velocity setpoints in seconds.
* @return The computed feedforward, in volts.
*/
units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
Vectord<1> r{currentVelocity.value()};
Vectord<1> nextR{nextVelocity.value()};
return kS * wpi::sgn(currentVelocity.value()) +
units::volt_t{feedforward.Calculate(r, nextR)(0)};
constexpr units::volt_t Calculate(
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
if (kA == decltype(kA)(0)) {
// Given the following discrete feedforward model
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
//
// We want the feedforward model when kₐ = 0.
//
// Simplify A.
//
// A = −kᵥ/kₐ
//
// As kₐ approaches zero, A approaches -∞.
//
// A = −∞
//
// Simplify A_d.
//
// A_d = eᴬᵀ
// A_d = std::exp(−∞)
// A_d = 0
//
// Simplify B_d.
//
// B_d = A⁻¹(eᴬᵀ - I)B
// B_d = A⁻¹((0) - I)B
// B_d = A⁻¹(-I)B
// B_d = -A⁻¹B
// B_d = -(−kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = (kᵥ/kₐ)⁻¹(1/kₐ)
// B_d = kₐ/kᵥ(1/kₐ)
// B_d = 1/kᵥ
//
// Substitute these into the feedforward equation.
//
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
// uₖ = (1/kᵥ)⁺(rₖ₊₁ − (0) rₖ)
// uₖ = kᵥrₖ₊₁
return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
} else {
// uₖ = B_d⁺(rₖ₊₁ − A_d rₖ)
//
// where
//
// A_d = eᴬᵀ
// B_d = A⁻¹(eᴬᵀ - I)B
// A = −kᵥ/kₐ
// B = 1/kₐ
double A = -kV.value() / kA.value();
double B = 1.0 / kA.value();
double A_d = gcem::exp(A * m_dt.value());
double B_d = 1.0 / A * (A_d - 1.0) * B;
return kS * wpi::sgn(currentVelocity) +
units::volt_t{
1.0 / B_d *
(nextVelocity.value() - A_d * currentVelocity.value())};
}
}
// Rearranging the main equation from the calculate() method yields the
@@ -168,5 +253,10 @@ class SimpleMotorFeedforward {
/** The acceleration gain. */
const units::unit_t<ka_unit> kA;
private:
/** The period. */
units::second_t m_dt;
};
} // namespace frc

15
wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View File

@@ -4,12 +4,14 @@
package edu.wpi.first.math.controller;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.units.MutableMeasure;
import org.junit.jupiter.api.Test;
class SimpleMotorFeedforwardTest {
@@ -24,22 +26,27 @@ class SimpleMotorFeedforwardTest {
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);
var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka, dt);
var r = VecBuilder.fill(2.0);
var nextR = VecBuilder.fill(3.0);
var currentVelocity = MutableMeasure.ofBaseUnits(2.0, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(3.0, RadiansPerSecond);
assertEquals(37.52499583432516 + 0.5, simpleMotor.calculate(2.0, 3.0, dt), 0.002);
assertEquals(
37.52499583432516 + 0.5,
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(2.0, 3.0, dt),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
assertEquals(
plantInversion.calculate(r, nextR).get(0, 0) + Ks,
simpleMotor.calculate(2.0, 1.0 / dt),
simpleMotor.calculate(currentVelocity, nextVelocity).magnitude(),
2.0);
}
}

36
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -4,6 +4,8 @@
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -41,33 +43,49 @@ class DifferentialDriveVoltageConstraintTest {
point.velocityMetersPerSecond,
0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
var acceleration = point.accelerationMetersPerSecondSq;
// Not really a strictly-correct test as we're using the chassis accel instead of the
// wheel accel, but much easier than doing it "properly" and a reasonable check anyway
assertAll(
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.leftMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.leftMetersPerSecond + dt * acceleration))
.in(Volts)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
feedforward
.calculate(
MetersPerSecond.of(wheelSpeeds.rightMetersPerSecond),
MetersPerSecond.of(
wheelSpeeds.rightMetersPerSecond + dt * acceleration))
.in(Volts)
>= -maxVoltage - 0.05));
}
}

11
wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java
View File

@@ -4,19 +4,21 @@
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.units.MutableMeasure;
import java.util.List;
import org.junit.jupiter.api.Test;
class ExponentialProfileTest {
private static final double kDt = 0.01;
private static final SimpleMotorFeedforward feedforward =
new SimpleMotorFeedforward(0, 2.5629, 0.43277);
new SimpleMotorFeedforward(0, 2.5629, 0.43277, kDt);
private static final ExponentialProfile.Constraints constraints =
ExponentialProfile.Constraints.fromCharacteristics(12, 2.5629, 0.43277);
@@ -43,10 +45,11 @@ class ExponentialProfileTest {
private static ExponentialProfile.State checkDynamics(
ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) {
var next = profile.calculate(kDt, current, goal);
var currentVelocity = MutableMeasure.ofBaseUnits(current.velocity, RadiansPerSecond);
var nextVelocity = MutableMeasure.ofBaseUnits(next.velocity, RadiansPerSecond);
var signal = feedforward.calculate(currentVelocity, nextVelocity);
var signal = feedforward.calculate(current.velocity, next.velocity, kDt);
assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9);
assertTrue(Math.abs(signal.magnitude()) < constraints.maxInput + 1e-9);
return next;
}

33
wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
View File

@@ -12,35 +12,34 @@
#include "units/acceleration.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
namespace frc {
TEST(SimpleMotorFeedforwardTest, Calculate) {
double Ks = 0.5;
double Kv = 3.0;
double Ka = 0.6;
auto dt = 0.02_s;
constexpr auto Ks = 0.5_V;
constexpr auto Kv = 3_V / 1_mps;
constexpr auto Ka = 0.6_V / 1_mps_sq;
constexpr units::second_t dt = 20_ms;
Matrixd<1, 1> A{-Kv / Ka};
Matrixd<1, 1> B{1.0 / Ka};
constexpr Matrixd<1, 1> A{{-Kv.value() / Ka.value()}};
constexpr Matrixd<1, 1> B{{1.0 / Ka.value()}};
frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{
units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
units::volt_t{Ka} / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> simpleMotor{Ks, Kv, Ka};
Vectord<1> r{2.0};
Vectord<1> nextR{3.0};
constexpr Vectord<1> r{{2.0}};
constexpr Vectord<1> nextR{{3.0}};
EXPECT_NEAR(37.524995834325161 + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
EXPECT_NEAR((37.524995834325161_V + Ks).value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 0.002);
// These won't match exactly. It's just an approximation to make sure they're
// in the same ballpark.
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0);
EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value(),
simpleMotor.Calculate(2_mps, 3_mps).value(), 2.0);
}
} // namespace frc

12
wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
View File

@@ -45,17 +45,19 @@ TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
point.velocity * point.curvature};
auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
auto acceleration = point.acceleration;
// Not really a strictly-correct test as we're using the chassis accel
// instead of the wheel accel, but much easier than doing it "properly" and
// a reasonable check anyway
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
EXPECT_TRUE(feedforward.Calculate(left, left + acceleration * dt) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
EXPECT_TRUE(feedforward.Calculate(left, left + acceleration * dt) >
-maxVoltage - 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
EXPECT_TRUE(feedforward.Calculate(right,
right + acceleration * dt) <
maxVoltage + 0.05_V);
EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
EXPECT_TRUE(feedforward.Calculate(right, right + acceleration * dt) >
-maxVoltage - 0.05_V);
}
}

52
wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp
View File

@@ -41,9 +41,9 @@ frc::ExponentialProfile<units::meter, units::volts>::State CheckDynamics(
frc::ExponentialProfile<units::meter, units::volts>::State current,
frc::ExponentialProfile<units::meter, units::volts>::State goal) {
auto next = profile.Calculate(kDt, current, goal);
auto signal = feedforward.Calculate(current.velocity, next.velocity, kDt);
auto signal = feedforward.Calculate(current.velocity, next.velocity);
EXPECT_LE(units::math::abs(signal), constraints.maxInput + 1e-9_V);
EXPECT_LE(units::math::abs(signal), (constraints.maxInput + 1e-9_V));
return next;
}
@@ -52,8 +52,8 @@ TEST(ExponentialProfileTest, ReachesGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -69,8 +69,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChange) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -92,8 +92,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -114,8 +114,8 @@ TEST(ExponentialProfileTest, Backwards) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -129,8 +129,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-10_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -151,8 +151,8 @@ TEST(ExponentialProfileTest, TopSpeed) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -172,8 +172,8 @@ TEST(ExponentialProfileTest, TopSpeedBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state;
@@ -193,8 +193,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 8_mps};
@@ -210,8 +210,8 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-40_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, -8_mps};
@@ -278,8 +278,8 @@ TEST(ExponentialProfileTest, TimingToCurrent) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -295,8 +295,8 @@ TEST(ExponentialProfileTest, TimingToGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};
@@ -318,8 +318,8 @@ TEST(ExponentialProfileTest, TimingToNegativeGoal) {
frc::ExponentialProfile<units::meter, units::volts>::Constraints constraints{
12_V, -kV / kA, 1 / kA};
frc::ExponentialProfile<units::meter, units::volts> profile{constraints};
frc::SimpleMotorFeedforward<units::meter> feedforward{0_V, 2.5629_V / 1_mps,
0.43277_V / 1_mps_sq};
frc::SimpleMotorFeedforward<units::meter> feedforward{
0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt};
frc::ExponentialProfile<units::meter, units::volts>::State goal{-2_m, 0_mps};
frc::ExponentialProfile<units::meter, units::volts>::State state{0_m, 0_mps};