From 30c7632ab8c7d69f6344440cc4dc381b1632bcc0 Mon Sep 17 00:00:00 2001 From: Nicholas Armstrong Date: Tue, 16 Jul 2024 20:23:11 -0400 Subject: [PATCH] [wpimath] Make SimpleMotorFeedforward only support discrete feedforward (#6647) Co-authored-by: Tyler Veness --- .../command/MecanumControllerCommand.java | 80 +++++---- .../wpilibj2/command/RamseteCommand.java | 25 ++- .../frc2/command/MecanumControllerCommand.cpp | 19 +-- .../cpp/frc2/command/RamseteCommand.cpp | 11 +- .../ElevatorExponentialProfile/cpp/Robot.cpp | 3 +- .../main/cpp/examples/EventLoop/cpp/Robot.cpp | 7 +- .../FlywheelBangBangController/cpp/Robot.cpp | 1 + .../cpp/subsystems/ShooterSubsystem.cpp | 1 + .../differentialdrivebot/Drivetrain.java | 9 +- .../Drivetrain.java | 9 +- .../subsystems/DriveSubsystem.java | 7 +- .../elevatorexponentialprofile/Robot.java | 5 +- .../elevatortrapezoidprofile/Robot.java | 5 +- .../wpilibj/examples/eventloop/Robot.java | 13 +- .../flywheelbangbangcontroller/Robot.java | 6 +- .../subsystems/ShooterSubsystem.java | 9 +- .../examples/mecanumbot/Drivetrain.java | 15 +- .../mecanumdriveposeestimator/Drivetrain.java | 15 +- .../subsystems/Shooter.java | 16 +- .../Drivetrain.java | 9 +- .../examples/swervebot/SwerveModule.java | 12 +- .../SwerveModule.java | 11 +- .../examples/sysid/subsystems/Shooter.java | 4 +- .../controller/SimpleMotorFeedforward.java | 157 ++++++++++++++---- .../frc/controller/SimpleMotorFeedforward.h | 150 +++++++++++++---- .../SimpleMotorFeedforwardTest.java | 15 +- ...ifferentialDriveVoltageConstraintTest.java | 36 +++- .../trajectory/ExponentialProfileTest.java | 11 +- .../controller/SimpleMotorFeedforwardTest.cpp | 33 ++-- .../DifferentialDriveVoltageTest.cpp | 12 +- .../cpp/trajectory/ExponentialProfileTest.cpp | 52 +++--- 31 files changed, 540 insertions(+), 218 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index 06663c9a80..fa299a5118 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -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 m_currentWheelSpeeds; private final Consumer m_outputDriveVoltages; private final Consumer m_outputWheelSpeeds; - private MecanumDriveWheelSpeeds m_prevSpeeds; - private double m_prevTime; + private final MutableMeasure> m_prevFrontLeftSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_prevRearLeftSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_prevFrontRightSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_prevRearRightSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_frontLeftSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_rearLeftSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_frontRightSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> 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 diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 050e97048d..b73deba668 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -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 m_output; private DifferentialDriveWheelSpeeds m_prevSpeeds = new DifferentialDriveWheelSpeeds(); + private final MutableMeasure> m_prevLeftSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_prevRightSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> m_leftSpeedSetpoint = + MutableMeasure.zero(MetersPerSecond); + private final MutableMeasure> 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 diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 6059db4ece..c22ac99ce8 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -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(), diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index b78b6ad4c1..e0da8c8eaa 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -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( diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp index 5d65a5aa36..3be3379683 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorExponentialProfile/cpp/Robot.cpp @@ -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; } diff --git a/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp index c8e84fb0ee..a29d169952 100644 --- a/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/EventLoop/cpp/Robot.cpp @@ -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); }); diff --git a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp index 047f01a1d1..6763e7f97c 100644 --- a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include /** diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp index cff73dac44..297ccc1eaa 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp @@ -5,6 +5,7 @@ #include "subsystems/ShooterSubsystem.h" #include +#include #include "Constants.h" diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java index adc52e2ef6..b351d169db 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java @@ -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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 4fc38ca525..40fd72700a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index 59d700bb1e..e2a66a64e1 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -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)); } /** diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java index 3aed7b92af..1bd9c727c4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorexponentialprofile/Robot.java @@ -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; } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java index 938132ba00..d451c1377f 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -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); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java index 08de65c591..63efa63351 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/eventloop/Robot.java @@ -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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java index 3bd0542904..e473213187 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java @@ -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. */ diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java index f6f85f700a..a8f644d52e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java @@ -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 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java index c64c9b3958..e00abf6d30 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java @@ -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( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index a534ae31b0..a660c2b07b 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -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( diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java index d282c2062c..88d564beb3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Shooter.java @@ -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"); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java index a69839bc7e..eb911054a5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -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 = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index c168860c99..32ca8fdbbc 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index 8ee8f4b07b..f42e1f4399 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -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); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java index 322df8e139..94574424db 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/sysid/subsystems/Shooter.java @@ -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( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index eeb76db319..60accdadf2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -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 < zero. * @throws IllegalArgumentException for ka < zero. + * @throws IllegalArgumentException for period ≤ 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 < zero. + * @throws IllegalArgumentException for ka < 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 The velocity parameter either as distance or angle. + * @param setpoint The velocity setpoint. + * @return The computed feedforward. + */ + public > Measure calculate(Measure> setpoint) { + return calculate(setpoint, setpoint); + } + + /** + * Calculates the feedforward from the gains and setpoints assuming discrete control. + * + * @param 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 > Measure calculate( + Measure> currentVelocity, Measure> 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 diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index cc31fc1058..1ae1af3df3 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -4,16 +4,15 @@ #pragma once +#include #include -#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, - units::unit_t kA = units::unit_t(0)) - : kS(kS), kV(kV), kA(kA) { - if (kV.value() < 0) { + units::unit_t kA = units::unit_t(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{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{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{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{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, - units::unit_t acceleration = - units::unit_t(0)) const { + [[deprecated("Use the current/next velocity overload instead.")]] + constexpr units::volt_t Calculate( + units::unit_t velocity, + units::unit_t 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 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 currentVelocity, - units::unit_t nextVelocity, - units::second_t dt) const { - auto plant = LinearSystemId::IdentifyVelocitySystem(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 currentVelocity, + units::unit_t 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; + + private: + /** The period. */ + units::second_t m_dt; }; + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index 6277c030cf..9d501edf92 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -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(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); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 1f9d59b795..1cedcae1de 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -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)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java index e9c535e1fb..6a987d4744 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -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; } diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp index 7878011227..35d35b8f36 100644 --- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp @@ -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 simpleMotor{ - units::volt_t{Ks}, units::volt_t{Kv} / 1_mps, - units::volt_t{Ka} / 1_mps_sq}; + frc::SimpleMotorFeedforward 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 diff --git a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp index 5282638ad0..6c60385489 100644 --- a/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp @@ -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); } } diff --git a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp index 8df3aa1e4b..663ed3af09 100644 --- a/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp +++ b/wpimath/src/test/native/cpp/trajectory/ExponentialProfileTest.cpp @@ -41,9 +41,9 @@ frc::ExponentialProfile::State CheckDynamics( frc::ExponentialProfile::State current, frc::ExponentialProfile::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::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -69,8 +69,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChange) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -92,8 +92,8 @@ TEST(ExponentialProfileTest, PosContinousUnderVelChangeBackward) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -114,8 +114,8 @@ TEST(ExponentialProfileTest, Backwards) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-10_m, 0_mps}; frc::ExponentialProfile::State state; @@ -129,8 +129,8 @@ TEST(ExponentialProfileTest, SwitchGoalInMiddle) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-10_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -151,8 +151,8 @@ TEST(ExponentialProfileTest, TopSpeed) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{40_m, 0_mps}; frc::ExponentialProfile::State state; @@ -172,8 +172,8 @@ TEST(ExponentialProfileTest, TopSpeedBackward) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-40_m, 0_mps}; frc::ExponentialProfile::State state; @@ -193,8 +193,8 @@ TEST(ExponentialProfileTest, HighInitialSpeed) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{40_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 8_mps}; @@ -210,8 +210,8 @@ TEST(ExponentialProfileTest, HighInitialSpeedBackward) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-40_m, 0_mps}; frc::ExponentialProfile::State state{0_m, -8_mps}; @@ -278,8 +278,8 @@ TEST(ExponentialProfileTest, TimingToCurrent) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{2_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -295,8 +295,8 @@ TEST(ExponentialProfileTest, TimingToGoal) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{2_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps}; @@ -318,8 +318,8 @@ TEST(ExponentialProfileTest, TimingToNegativeGoal) { frc::ExponentialProfile::Constraints constraints{ 12_V, -kV / kA, 1 / kA}; frc::ExponentialProfile profile{constraints}; - frc::SimpleMotorFeedforward feedforward{0_V, 2.5629_V / 1_mps, - 0.43277_V / 1_mps_sq}; + frc::SimpleMotorFeedforward feedforward{ + 0_V, 2.5629_V / 1_mps, 0.43277_V / 1_mps_sq, kDt}; frc::ExponentialProfile::State goal{-2_m, 0_mps}; frc::ExponentialProfile::State state{0_m, 0_mps};